Improved hokyo time stamps

The hokuyo driver publishes more accurate time stamps now. The stamp is
calculated based on current system time and a calibrated offset that accounts
for latencies between measurement time and the actual time when a measurement
is received by the driver.

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

@@ -0,0 +1,22 @@
+package org.ros.android.hokuyo;
+
+import java.util.List;
+
+public class LaserScan {
+  private final double timeStamp;
+  private final List<Float> ranges;
+
+  public LaserScan(double timeStamp, List<Float> ranges) {
+    this.timeStamp = timeStamp;
+    this.ranges = ranges;
+  }
+
+  public double getTimeStamp() {
+    return timeStamp;
+  }
+
+  public List<Float> getRanges() {
+    return ranges;
+  }
+
+}

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

@@ -16,10 +16,8 @@
 
 package org.ros.android.hokuyo;
 
-import java.util.List;
-
 public interface LaserScanListener {
   
-  void onNewLaserScan(List<Float> ranges);
+  void onNewLaserScan(LaserScan scan);
 
 }

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

@@ -18,7 +18,6 @@ package org.ros.android.hokuyo;
 
 import org.ros.message.Duration;
 import org.ros.message.MessageListener;
-import org.ros.message.sensor_msgs.LaserScan;
 import org.ros.message.std_msgs.Time;
 import org.ros.node.DefaultNodeFactory;
 import org.ros.node.Node;
@@ -27,8 +26,6 @@ import org.ros.node.NodeMain;
 import org.ros.node.parameter.ParameterTree;
 import org.ros.node.topic.Publisher;
 
-import java.util.List;
-
 /**
  * @author damonkohler@google.com (Damon Kohler)
  */
@@ -39,8 +36,10 @@ public class LaserScanPublisher implements NodeMain {
 
   private final Scip20Device scipDevice;
 
+  private Duration timeOffset;
+  
   private Node node;
-  private Publisher<LaserScan> publisher;
+  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 
@@ -66,10 +65,13 @@ public class LaserScanPublisher implements NodeMain {
         });
     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(List<Float> ranges) {
-        LaserScan message = node.getMessageFactory().newMessage("sensor_msgs/LaserScan");
+      public void onNewLaserScan(LaserScan scan) {
+        org.ros.message.sensor_msgs.LaserScan message = 
+            node.getMessageFactory().newMessage("sensor_msgs/LaserScan");
         // Some laser scanners have blind areas before and after the actual detection range.
         // These are indicated by the front step and the last step configuration variables.
         // Since the blind values never change, we can just ignore them when creating the 
@@ -79,14 +81,14 @@ public class LaserScanPublisher implements NodeMain {
         message.angle_max = (configuration.getLastStep() - configuration.getFrontStep()) * message.angle_increment;
         message.ranges = new float[configuration.getLastStep() - configuration.getFirstStep()];
         for (int i = 0; i < message.ranges.length; i++) {
-          message.ranges[i] = (float) (ranges.get(i + configuration.getFirstStep()) / 1000.0);
+          message.ranges[i] = (float) (scan.getRanges().get(i + configuration.getFirstStep()) / 1000.0);
         }
         message.time_increment = (float) (60.0 / ((double) configuration.getStandardMotorSpeed() * configuration.getTotalSteps()));
         message.scan_time = (float) (60.0 * (SKIP + 1) / (double) configuration.getStandardMotorSpeed());
         message.range_min = (float) (configuration.getMinimumMeasurment() / 1000.0);
         message.range_max = (float) (configuration.getMaximumMeasurement() / 1000.0);
         message.header.frame_id = laserFrame;
-        message.header.stamp = node.getCurrentTime().add(timeOffset);
+        message.header.stamp = new org.ros.message.Time(scan.getTimeStamp()).add(timeOffset);
         publisher.publish(message);
       }
     });

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

@@ -31,6 +31,7 @@ import java.io.InputStreamReader;
 import java.io.OutputStream;
 import java.io.OutputStreamWriter;
 import java.nio.charset.Charset;
+import java.util.Arrays;
 
 /**
  * @author damonkohler@google.com (Damon Kohler)
@@ -44,6 +45,8 @@ public class Scip20Device {
 
   private final BufferedReader reader;
   private final BufferedWriter writer;
+  
+  private double scanOffset;
 
   /**
    * It is not necessary to provide buffered streams. Buffering is handled
@@ -146,8 +149,12 @@ public class Scip20Device {
     Preconditions.checkState(read().length() == 0);
   }
 
-  private String readTimestamp() {
-    return verifyChecksum(read());
+  private long readTimestamp() {
+    String stampString = verifyChecksum(read());
+    long stamp = 0;
+    for(int i=0; i<4; i++)
+      stamp |= stampString.charAt(i) << i;
+    return stamp;
   }
 
   public void startScanning(final LaserScanListener listener) {
@@ -160,13 +167,14 @@ public class Scip20Device {
         checkTerminator();
         while (true) {
           Preconditions.checkState(read().equals(command));
+          double scanStartTime = System.currentTimeMillis() / 1000.0;
           checkStatus();
           readTimestamp();
           StringBuilder data = new StringBuilder();
           while (true) {
             String line = read(); // Data and checksum or terminating LF
             if (line.length() == 0) {
-              listener.onNewLaserScan(Decoder.decode(data.toString(), 3));
+              listener.onNewLaserScan(new LaserScan(scanStartTime + scanOffset, Decoder.decode(data.toString(), 3)));
               break;
             }
             data.append(verifyChecksum(line));
@@ -212,4 +220,72 @@ public class Scip20Device {
       e.printStackTrace();
     }
   }
+  
+  // TODO(moesenle): assert that scanning is not running
+  public 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
+     *   3. request hokuyo time again and calculate offset to current time
+     *   4. offset = scan - (end + start)/2
+     * We repeat this process 11 times and take the median offset.
+     */
+    long[] samples = new long[11];
+    long start = hokuyoClockOffset();
+    for(int i=0; i<samples.length; i++) {
+      long scan = hokuyoScanOffset();
+      long end = hokuyoClockOffset();
+      samples[i] = scan - (end + start) / 2;
+      start = end;
+    }
+    Arrays.sort(samples);
+    scanOffset = samples[5] / 1000.0;
+  }
+  
+  public double getScanOffset() {
+    return scanOffset;
+  }
+  
+  private long hokuyoClockOffset() {
+    // Enter time adjust mode
+    write("TM0");
+    checkStatus();
+    checkTerminator();
+
+    // Read the current time stamp
+    final long start = System.currentTimeMillis();
+    write("TM1");
+    checkStatus();
+    final long offset = readTimestamp()
+        - (start + System.currentTimeMillis()) / 2;
+    checkTerminator();
+    
+    // Leave adjust mode
+    write("TM2");
+    checkStatus();
+    checkTerminator();
+    
+    return offset;
+  }
+  
+  private long hokuyoScanOffset() {
+    // We request exactly one scan from the laser and use the difference 
+    // between the laser's own time stamp and the system time at which we
+    // received the scan
+    write("MD0000076800001");
+    checkStatus();
+    checkTerminator();
+    
+    Preconditions.checkState(read().equals("MD0000076800000"));
+    long scanStartTime = System.currentTimeMillis();
+    checkStatus();
+    long scanTimeStamp = readTimestamp();
+    while(true) {
+        String line = read(); // Data and checksum or terminating LF
+        if (line.length() == 0) 
+          break;
+        verifyChecksum(line);
+    }
+    return scanTimeStamp - scanStartTime;
+  }
 }