Some refactoring: extracted interfaces for laser configuration and laser device

android_hokuyo/src/org/ros/android/hokuyo/LaserScanPublisher.java

@@ -31,54 +31,57 @@ import org.ros.node.topic.Publisher;
  */
 public class LaserScanPublisher implements NodeMain {
 
-  private final Scip20Device scipDevice;
+  private final LaserScannerDevice scipDevice;
+
+  /**
+   * The offset from our local clock to the actual wall clock time. This is
+   * necessary because we cannot set time on android devices accurately enough
+   * at the moment.
+   */
+  private Duration wallClockOffset;
 
-  private Duration timeOffset;
-  
   private Node node;
   private Publisher<org.ros.message.sensor_msgs.LaserScan> publisher;
-  
+
   /**
-   * We need a way to adjust time stamps because it is not (easily) possible to change 
-   * a tablet's clock.
+   * We need a way to adjust time stamps because it is not (easily) possible to
+   * change a tablet's clock.
    */
-  public LaserScanPublisher(Scip20Device scipDevice) {
+  public LaserScanPublisher(LaserScannerDevice scipDevice) {
     this.scipDevice = scipDevice;
   }
 
   @Override
   public void main(NodeConfiguration nodeConfiguration) throws Exception {
-    node = new DefaultNodeFactory().newNode("android_hokuyo_node", nodeConfiguration);
+    node = new DefaultNodeFactory().newNode("android_hokuyo_node",
+        nodeConfiguration);
     ParameterTree params = node.newParameterTree();
     final String laserTopic = params.getString("~laser_topic", "laser");
     final String laserFrame = params.getString("~laser_frame", "laser");
-    timeOffset = new Duration(0);
-    publisher = node.newPublisher(node.resolveName(laserTopic), "sensor_msgs/LaserScan");
-    node.newSubscriber("/wall_clock", "std_msgs/Time", 
+    wallClockOffset = new Duration(0);
+    publisher = node.newPublisher(node.resolveName(laserTopic),
+        "sensor_msgs/LaserScan");
+    node.newSubscriber("/wall_clock", "std_msgs/Time",
         new MessageListener<org.ros.message.std_msgs.Time>() {
           @Override
           public void onNewMessage(Time message) {
-            timeOffset = message.data.subtract(node.getCurrentTime());
+            wallClockOffset = message.data.subtract(node.getCurrentTime());
           }
         });
-    scipDevice.reset();
-    final Configuration configuration = scipDevice.queryConfiguration();
-    scipDevice.calibrateTime();
-    node.getLog().info("Calibrated laser time offset: " + scipDevice.getScanOffset());
     scipDevice.startScanning(new LaserScanListener() {
       @Override
       public void onNewLaserScan(LaserScan scan) {
         org.ros.message.sensor_msgs.LaserScan message = toLaserScanMessage(
-            laserFrame, configuration, scan);
+            laserFrame, scan);
         publisher.publish(message);
-      }});
+      }
+    });
   }
 
   @Override
   public void shutdown() {
     scipDevice.shutdown();
   }
-  
 
   /**
    * Construct a LaserScan message from sensor readings and the laser
@@ -93,16 +96,16 @@ public class LaserScanPublisher implements NodeMain {
    * 
    * @param laserFrame
    *          The laser's sensor frame.
-   * @param configuration
-   *          The laser's current configuration.
    * @param scan
    *          The actual range readings.
    * @return A new LaserScan message
    */
   private org.ros.message.sensor_msgs.LaserScan toLaserScanMessage(
-      String laserFrame, Configuration configuration, LaserScan scan) {
+      String laserFrame, LaserScan scan) {
     org.ros.message.sensor_msgs.LaserScan message = node.getMessageFactory()
         .newMessage("sensor_msgs/LaserScan");
+    LaserScannerConfiguration configuration = scipDevice.getConfiguration();
+    
     message.angle_increment = configuration.getAngleIncrement();
     message.angle_min = configuration.getMinimumAngle();
     message.angle_max = configuration.getMaximumAngle();
@@ -118,7 +121,7 @@ public class LaserScanPublisher implements NodeMain {
     message.range_max = (float) (configuration.getMaximumMeasurement() / 1000.0);
     message.header.frame_id = laserFrame;
     message.header.stamp = new org.ros.message.Time(scan.getTimeStamp())
-        .add(timeOffset);
+        .add(wallClockOffset);
     return message;
   }
 

android_hokuyo/src/org/ros/android/hokuyo/LaserScannerConfiguration.java

@@ -0,0 +1,82 @@
+package org.ros.android.hokuyo;
+
+public interface LaserScannerConfiguration {
+
+  /**
+   * @return The laser's model.
+   */
+  String getModel();
+
+  /**
+   * @return The minimal range.
+   */
+  int getMinimumMeasurment();
+
+  /**
+   * @return The maximal range.
+   */
+  int getMaximumMeasurement();
+
+  /**
+   * @return The total number of range readings returned by the laser.
+   */
+  int getTotalSteps();
+
+  /**
+   * Returns the first meaningful range reading. The laser might have a blind
+   * area at the beginning of the scan range. Range readings are generated for
+   * this area, they do not contain any useful information though.
+   * 
+   * @return The index of the first meaningful range reading.
+   */
+  int getFirstStep();
+
+  /**
+   * Returns the last meaningful range reading. The laser might have a blind
+   * area at the end of the scan range. Range readings are generated for this
+   * area, they do not contain any useful information though.
+   * 
+   * @return The index of the last meaningful range reading.
+   */
+  int getLastStep();
+
+  /**
+   * Returns the front step of the laser. The front step is the index of the
+   * reading that is pointing directly forward.
+   * 
+   * @return The index of the front step.
+   */
+  int getFrontStep();
+
+  /**
+   * @return The motor speed of the laser
+   */
+  int getStandardMotorSpeed();
+
+  /**
+   * @return The angle increment i.e. the angle between two successive points in
+   *         a scan.
+   */
+  float getAngleIncrement();
+
+  /**
+   * @return The minimum angle, i.e. the angle of the first step
+   */
+  float getMinimumAngle();
+
+  /**
+   * @return The maximum angle, i.e. the angle of the last step
+   */
+  float getMaximumAngle();
+
+  /**
+   * @return The time increment between two successive points in a scan.
+   */
+  float getTimeIncrement();
+
+  /**
+   * @return The time between two scans.
+   */
+  float getScanTime();
+
+}

android_hokuyo/src/org/ros/android/hokuyo/LaserScannerDevice.java

@@ -0,0 +1,11 @@
+package org.ros.android.hokuyo;
+
+public interface LaserScannerDevice {
+
+  void startScanning(final LaserScanListener listener);
+
+  void shutdown();
+
+  LaserScannerConfiguration getConfiguration();
+
+}

android_hokuyo/src/org/ros/android/hokuyo/Scip20Device.java

@@ -36,7 +36,7 @@ import java.util.Arrays;
 /**
  * @author damonkohler@google.com (Damon Kohler)
  */
-public class Scip20Device {
+public class Scip20Device implements LaserScannerDevice {
 
   private static final boolean DEBUG = false;
   private static final String TAG = "Scip20Device";
@@ -45,8 +45,12 @@ public class Scip20Device {
 
   private final BufferedReader reader;
   private final BufferedWriter writer;
+  private final LaserScannerConfiguration configuration;
   
-  private double scanOffset;
+  /**
+   * The time offset between the first 
+   */
+  private double scanTimeOffset;
 
   /**
    * It is not necessary to provide buffered streams. Buffering is handled
@@ -68,6 +72,10 @@ public class Scip20Device {
     writer =
         new BufferedWriter(new OutputStreamWriter(new BufferedOutputStream(outputStream,
             STREAM_BUFFER_SIZE), Charset.forName("US-ASCII")));
+    
+    reset();
+    configuration = queryConfiguration();
+    calibrateTime();
   }
 
   private void write(String command) {
@@ -121,7 +129,7 @@ public class Scip20Device {
     throw new InvalidChecksum();
   }
 
-  public void reset() {
+  private void reset() {
     try {
       write("RS");
       checkStatus();
@@ -153,6 +161,7 @@ public class Scip20Device {
     return Decoder.decodeValue(verifyChecksum(read()), 4);
   }
 
+  @Override
   public void startScanning(final LaserScanListener listener) {
     new Thread() {
       @Override
@@ -170,7 +179,7 @@ public class Scip20Device {
           while (true) {
             String line = read(); // Data and checksum or terminating LF
             if (line.length() == 0) {
-              listener.onNewLaserScan(new LaserScan(scanStartTime + scanOffset, Decoder.decodeValues(data.toString(), 3)));
+              listener.onNewLaserScan(new LaserScan(scanStartTime + scanTimeOffset, Decoder.decodeValues(data.toString(), 3)));
               break;
             }
             data.append(verifyChecksum(line));
@@ -186,8 +195,8 @@ public class Scip20Device {
     return buffer.substring(0, buffer.length() - 2) + buffer.charAt(buffer.length() - 1);
   }
 
-  public Configuration queryConfiguration() {
-    Configuration.Builder builder = new Configuration.Builder();
+  private LaserScannerConfiguration queryConfiguration() {
+    Scip20DeviceConfiguration.Builder builder = new Scip20DeviceConfiguration.Builder();
     write("PP");
     checkStatus();
     builder.parseModel(verifyChecksum(readAndStripSemicolon()));
@@ -202,6 +211,7 @@ public class Scip20Device {
     return builder.build();
   }
 
+  @Override
   public void shutdown() {
     try {
       reader.close();
@@ -218,7 +228,7 @@ public class Scip20Device {
   }
   
   // TODO(moesenle): assert that scanning is not running
-  public void calibrateTime() {
+  private void calibrateTime() {
     /* To calibrate time, we do the following (similar to what ROS' hokuyo_node does):
      *   1. get current hokuyo time and calculate offset to current time
      *   2. request a scan and calculate the scan offset to current time
@@ -235,11 +245,7 @@ public class Scip20Device {
       start = end;
     }
     Arrays.sort(samples);
-    scanOffset = samples[5] / 1000.0;
-  }
-  
-  public double getScanOffset() {
-    return scanOffset;
+    scanTimeOffset = samples[5] / 1000.0;
   }
   
   private long hokuyoClockOffset() {
@@ -284,4 +290,9 @@ public class Scip20Device {
     }
     return scanTimeStamp - scanStartTime;
   }
+
+  @Override
+  public LaserScannerConfiguration getConfiguration() {
+    return configuration;
+  }
 }

android_hokuyo/src/org/ros/android/hokuyo/Configuration.java → android_hokuyo/src/org/ros/android/hokuyo/Scip20DeviceConfiguration.java

@@ -22,7 +22,7 @@ import com.google.common.base.Preconditions;
 /**
  * @author damonkohler@google.com (Damon Kohler)
  */
-public class Configuration {
+public class Scip20DeviceConfiguration implements LaserScannerConfiguration {
 
   private String model;
   private int minimumMeasurment; // mm
@@ -35,13 +35,13 @@ public class Configuration {
 
   public static class Builder {
 
-    private Configuration configuration;
+    private Scip20DeviceConfiguration configuration;
 
     public Builder() {
-      configuration = new Configuration();
+      configuration = new Scip20DeviceConfiguration();
     }
 
-    public Configuration build() {
+    public LaserScannerConfiguration build() {
       return configuration;
     }
 
@@ -94,34 +94,38 @@ public class Configuration {
 
   }
 
-  private Configuration() {
+  private Scip20DeviceConfiguration() {
     // Use the Configuration.Builder to construct a Configuration object.
   }
 
   /**
-   * @return The laser's model.
+   * @return the laser's model
    */
+  @Override
   public String getModel() {
     return model;
   }
 
   /**
-   * @return The minimal range.
+   * @return the minimal range
    */
+  @Override
   public int getMinimumMeasurment() {
     return minimumMeasurment;
   }
 
   /**
-   * @return The maximal range.
+   * @return the maximal range
    */
+  @Override
   public int getMaximumMeasurement() {
     return maximumMeasurement;
   }
 
   /**
-   * @return The total number of range readings returned by the laser.
+   * @return the total number of range readings returned by the laser
    */
+  @Override
   public int getTotalSteps() {
     return totalSteps;
   }
@@ -131,8 +135,9 @@ public class Configuration {
    * area at the beginning of the scan range. Range readings are generated for
    * this area, they do not contain any useful information though.
    * 
-   * @return The index of the first meaningful range reading.
+   * @return the index of the first meaningful range reading
    */
+  @Override
   public int getFirstStep() {
     return firstStep;
   }
@@ -142,8 +147,9 @@ public class Configuration {
    * area at the end of the scan range. Range readings are generated for this
    * area, they do not contain any useful information though.
    * 
-   * @return The index of the last meaningful range reading.
+   * @return the index of the last meaningful range reading
    */
+  @Override
   public int getLastStep() {
     return lastStep;
   }
@@ -152,51 +158,58 @@ public class Configuration {
    * Returns the front step of the laser. The front step is the index of the
    * reading that is pointing directly forward.
    * 
-   * @return The index of the front step.
+   * @return the index of the front step
    */
+  @Override
   public int getFrontStep() {
     return frontStep;
   }
 
   /**
-   * @return The motor speed of the laser
+   * @return the motor speed of the laser
    */
+  @Override
   public int getStandardMotorSpeed() {
     return standardMotorSpeed;
   }
 
   /**
-   * @return The angle increment i.e. the angle between two successive points in
-   *         a scan.
+   * @return the angle increment i.e. the angle between two successive points in
+   *         a scan
    */
+  @Override
   public float getAngleIncrement() {
     return (float) ((2.0 * Math.PI) / getTotalSteps());
   }
 
   /**
-   * @return The minimum angle, i.e. the angle of the first step
+   * @return the minimum angle, i.e. the angle of the first step
    */
+  @Override
   public float getMinimumAngle() {
     return (getFirstStep() - getFrontStep()) * getAngleIncrement();
   }
 
   /**
-   * @return The maximum angle, i.e. the angle of the last step
+   * @return the maximum angle, i.e. the angle of the last step
    */
+  @Override
   public float getMaximumAngle() {
     return (getLastStep() - getFrontStep()) * getAngleIncrement();
   }
 
   /**
-   * @return The time increment between two successive points in a scan.
+   * @return the time increment between two successive points in a scan
    */
+  @Override
   public float getTimeIncrement() {
     return (float) (60.0 / ((double) getStandardMotorSpeed() * getTotalSteps()));
   }
 
   /**
-   * @return The time between two scans.
+   * @return the time between two scans
    */
+  @Override
   public float getScanTime() {
     return (float) (60.0 / (double) getStandardMotorSpeed());
   }