merge

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

@@ -18,6 +18,8 @@ package org.ros.android.hokuyo;
 
 import java.util.List;
 
+import android.gesture.Prediction;
+
 import com.google.common.base.Preconditions;
 import com.google.common.collect.Lists;
 
@@ -25,8 +27,10 @@ class Decoder {
 
   public static int decodeValue(String buffer, int blockSize) {
     Preconditions.checkArgument(buffer.length() == blockSize);
+    Preconditions.checkArgument(blockSize == 2 || blockSize == 3
+        || blockSize == 4);
     int result = 0;
-    for(int i = 0; i < blockSize; i++) {
+    for (int i = 0; i < blockSize; i++) {
       result |= (buffer.charAt(blockSize - i - 1) - 0x30) << i * 6;
     }
     return result;

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

@@ -86,17 +86,22 @@ public class LaserScanPublisher implements NodeMain {
    *          The actual range readings.
    * @return A new LaserScan message
    */
-  private org.ros.message.sensor_msgs.LaserScan toLaserScanMessage(
+  @VisibleForTesting
+  org.ros.message.sensor_msgs.LaserScan toLaserScanMessage(
       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();
     message.ranges = new float[configuration.getLastStep()
-        - configuration.getFirstStep()];
+        - configuration.getFirstStep() + 1];
+    Preconditions
+        .checkState(message.ranges.length <= scan.getRanges().size(),
+            String.format("Number of scans in configuration does not match received range measurements (%d > %d).", 
+                message.ranges.length, scan.getRanges().size()));
     for (int i = 0; i < message.ranges.length; i++) {
       message.ranges[i] = (float) (scan.getRanges().get(
           i + configuration.getFirstStep()) / 1000.0);
@@ -109,5 +114,9 @@ public class LaserScanPublisher implements NodeMain {
     message.header.stamp = node.getCurrentTime();
     return message;
   }
-
+  
+  @VisibleForTesting
+  void setNode(Node node) {
+    this.node = node;
+  }
 }

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

@@ -16,11 +16,6 @@
 
 package org.ros.android.hokuyo;
 
-import com.google.common.base.Preconditions;
-
-import android.util.Log;
-import org.ros.exception.RosRuntimeException;
-
 import java.io.BufferedInputStream;
 import java.io.BufferedOutputStream;
 import java.io.BufferedReader;
@@ -31,13 +26,22 @@ import java.io.InputStreamReader;
 import java.io.OutputStream;
 import java.io.OutputStreamWriter;
 import java.nio.charset.Charset;
-import java.util.Arrays;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+import org.ros.exception.RosRuntimeException;
+
+import android.util.Log;
+
+import com.google.common.base.Preconditions;
 
 /**
  * @author damonkohler@google.com (Damon Kohler)
  */
 public class Scip20Device implements LaserScannerDevice {
 
+  private static final int TIME_CALIBRATION_AVERAGING_COUNT = 11;
   private static final boolean DEBUG = false;
   private static final String TAG = "Scip20Device";
 
@@ -48,10 +52,27 @@ public class Scip20Device implements LaserScannerDevice {
   private final LaserScannerConfiguration configuration;
 
   /**
-   * The time offset between the first
+   * The time offset between taking the scan and the USB package arriving.
    */
   private double scanTimeOffset;
 
+  /**
+   * Calculates the median. This method modifies the sequence.
+   * 
+   * @param sequence
+   *          input data to get the median from, this parameter is modified
+   * @return the median
+   */
+  private static <T extends Comparable<? super T>> T calculateMedian(
+      List<? extends T> sequence) {
+    Collections.sort(sequence);
+    if (sequence.size() % 2 == 0) {
+      return sequence.get(sequence.size() / 2);
+    } else {
+      return sequence.get(sequence.size() / 2 + 1);
+    }
+  }
+
   /**
    * It is not necessary to provide buffered streams. Buffering is handled
    * internally.
@@ -196,7 +217,8 @@ public class Scip20Device implements LaserScannerDevice {
   private String readAndStripSemicolon() {
     String buffer = read();
     Preconditions.checkState(buffer.charAt(buffer.length() - 2) == ';');
-    return buffer.substring(0, buffer.length() - 2) + buffer.charAt(buffer.length() - 1);
+    return buffer.substring(0, buffer.length() - 2)
+        + buffer.charAt(buffer.length() - 1);
   }
 
   private LaserScannerConfiguration queryConfiguration() {
@@ -243,17 +265,17 @@ public class Scip20Device implements LaserScannerDevice {
    * We repeat this process 11 times and take the median offset.
    */
   private void calibrateTime() {
-    // TODO(moesenle): Assert that scanning is not running.
-    long[] samples = new long[11];
+    ArrayList<Long> samples = new ArrayList<Long>(
+        TIME_CALIBRATION_AVERAGING_COUNT);
     long start = calculateClockOffset();
-    for (int i = 0; i < samples.length; i++) {
+    for (int i = 0; i < samples.size(); i++) {
       long scan = calculateScanTimeOffset();
-      long end = calculateClockOffset();
-      samples[i] = scan - (end + start) / 2;
+      long end = calculateScanTimeOffset();
+      samples.set(i, scan - (end + start) / 2);
       start = end;
     }
-    Arrays.sort(samples);
-    scanTimeOffset = samples[5] / 1000.0;
+
+    scanTimeOffset = calculateMedian(samples) / 1000.0;
   }
 
   private long calculateClockOffset() {
@@ -266,7 +288,7 @@ public class Scip20Device implements LaserScannerDevice {
     final long start = System.currentTimeMillis();
     write("TM1");
     checkStatus();
-    final long offset = readTimestamp() - (start + System.currentTimeMillis()) / 2;
+    long offset = readTimestamp() - (start + System.currentTimeMillis()) / 2;
     checkTerminator();
 
     // Leave adjust mode

android_hokuyo_test/manifest.xml

@@ -11,6 +11,7 @@
 
   <depend package="rosjava" />
   <depend package="android_hokuyo" />
+  <depend package="sensor_msgs" />
 
   <export>
     <rosjava-android-app target="android-13" />

android_hokuyo_test/src/org/ros/android/hokuyo/DecoderTest.java

@@ -23,8 +23,17 @@ import junit.framework.TestCase;
  */
 public class DecoderTest extends TestCase {
   
-  public void testDecode3Letter() {
+  public void testDecodeValue2() {
+    assertEquals(1234, Decoder.decodeValue("CB", 2));
+  }
+  
+  public void testDecodeValue3() {
     assertEquals(5432, Decoder.decodeValue("1Dh", 3));
   }
+  
+  public void testDecodeValue4() {
+    assertEquals(16000000, Decoder.decodeValue("m2@0", 4));
+  }
+
 
 }

android_hokuyo_test/src/org/ros/android/hokuyo/LaserScanPublisherIntegrationTest.java

@@ -0,0 +1,245 @@
+/*
+ * Copyright (C) 2011 Google Inc.
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ * 
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * 
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+package org.ros.android.hokuyo;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.CountDownLatch;
+import java.util.concurrent.TimeUnit;
+
+import junit.framework.TestCase;
+
+import org.ros.RosCore;
+import org.ros.internal.node.DefaultNode;
+import org.ros.message.MessageListener;
+import org.ros.message.sensor_msgs.LaserScan;
+import org.ros.node.DefaultNodeRunner;
+import org.ros.node.Node;
+import org.ros.node.NodeConfiguration;
+import org.ros.node.NodeMain;
+import org.ros.node.NodeRunner;
+import org.ros.node.topic.Subscriber;
+
+import com.google.common.annotations.VisibleForTesting;
+
+/**
+ * @author moesenle@google.com (Lorenz Moesenlechner)
+ */
+public class LaserScanPublisherIntegrationTest extends TestCase {
+
+  private class FakeLaserDevice implements LaserScannerDevice {
+
+    private static final int SCAN_PUBLISH_FREQUENCY = 10;
+
+    private class RepeatingScanGeneratorThread extends Thread {
+      private LaserScanListener listener;
+      private int frequency;
+
+      public RepeatingScanGeneratorThread(int frequency,
+          LaserScanListener listener) {
+        this.listener = listener;
+        this.frequency = frequency;
+      }
+
+      @Override
+      public void run() {
+        try {
+          while (!Thread.currentThread().isInterrupted()) {
+            listener.onNewLaserScan(makeFakeScan());
+            Thread.sleep((long) (1000f / frequency));
+          }
+        } catch (InterruptedException e) {
+          // Cancelable
+        }
+      }
+
+      public void cancel() {
+        interrupt();
+      }
+    }
+
+    private RepeatingScanGeneratorThread scanGeneratorThread;
+    private int numberOfRangeValues;
+
+    public FakeLaserDevice() {
+      numberOfRangeValues = 0;
+    }
+
+    public FakeLaserDevice(int numberOfRangeValues) {
+      this.numberOfRangeValues = numberOfRangeValues;
+    }
+
+    @Override
+    public void startScanning(LaserScanListener listener) {
+      if (scanGeneratorThread != null) {
+        scanGeneratorThread.cancel();
+      }
+      scanGeneratorThread = new RepeatingScanGeneratorThread(
+          SCAN_PUBLISH_FREQUENCY, listener);
+      scanGeneratorThread.start();
+    }
+
+    @Override
+    public void shutdown() {
+      if (scanGeneratorThread != null) {
+        scanGeneratorThread.cancel();
+      }
+    }
+
+    @Override
+    public LaserScannerConfiguration getConfiguration() {
+      return new FakeLaserScannerConfiguration();
+    }
+
+    @VisibleForTesting
+    org.ros.android.hokuyo.LaserScan makeFakeScan() {
+      List<Integer> fakeRangeMeasurements = new ArrayList<Integer>(
+          numberOfRangeValues);
+      for (int i = 0; i < numberOfRangeValues; i++) {
+        fakeRangeMeasurements.add(0);
+      }
+      return new org.ros.android.hokuyo.LaserScan(0.0, fakeRangeMeasurements);
+    }
+  }
+
+  private final class FakeLaserScannerConfiguration implements
+      LaserScannerConfiguration {
+    @Override
+    public String getModel() {
+      return "TestLaserScanner";
+    }
+
+    @Override
+    public int getMinimumMeasurment() {
+      return 0;
+    }
+
+    @Override
+    public int getMaximumMeasurement() {
+      return 1000;
+    }
+
+    @Override
+    public int getTotalSteps() {
+      return 3;
+    }
+
+    @Override
+    public int getFirstStep() {
+      return 0;
+    }
+
+    @Override
+    public int getLastStep() {
+      return 2;
+    }
+
+    @Override
+    public int getFrontStep() {
+      return 1;
+    }
+
+    @Override
+    public int getStandardMotorSpeed() {
+      return 0;
+    }
+
+    @Override
+    public float getAngleIncrement() {
+      return (float) Math.PI;
+    }
+
+    @Override
+    public float getMinimumAngle() {
+      return (float) -Math.PI;
+    }
+
+    @Override
+    public float getMaximumAngle() {
+      return (float) Math.PI;
+    }
+
+    @Override
+    public float getTimeIncrement() {
+      return 0;
+    }
+
+    @Override
+    public float getScanTime() {
+      return 0;
+    }
+  }
+
+  public void testLaserScanPublisher() throws InterruptedException {
+    RosCore core = RosCore.newPrivate();
+    core.start();
+    assertTrue(core.awaitStart(1, TimeUnit.SECONDS));
+
+    final NodeConfiguration nodeConfiguration = NodeConfiguration
+        .newPrivate(core.getUri());
+    final NodeRunner runner = DefaultNodeRunner.newDefault();
+    final CountDownLatch laserScanReceived = new CountDownLatch(1);
+    runner.run(new NodeMain() {
+      private FakeLaserDevice fakeLaser;
+      private Node node;
+
+      @Override
+      public void main(Node node) throws Exception {
+        this.node = node;
+        Subscriber<org.ros.message.sensor_msgs.LaserScan> laserScanSubscriber = node
+            .newSubscriber("laser", "sensor_msgs/LaserScan",
+                new MessageListener<org.ros.message.sensor_msgs.LaserScan>() {
+                  @Override
+                  public void onNewMessage(LaserScan message) {
+                    assertTrue(message.ranges.length == 3);
+                    laserScanReceived.countDown();
+                    // Check that fake laser data is equal to the received
+                    // message
+                  }
+                });
+        assertTrue(laserScanSubscriber.awaitRegistration(1, TimeUnit.SECONDS));
+        fakeLaser = new FakeLaserDevice(3);
+        runner.run(new LaserScanPublisher(fakeLaser),
+            nodeConfiguration.setNodeName("laser_scan_publisher_test"));
+      }
+
+      @Override
+      public void shutdown() {
+        fakeLaser.shutdown();
+        node.shutdown();
+      }
+    }, nodeConfiguration.setNodeName("android_hokuyo_test_node"));
+
+    assertTrue(laserScanReceived.await(1, TimeUnit.SECONDS));
+    runner.shutdown();
+  }
+
+  public void testLaserScannerInvalidNumberOfMeasurements() {
+    FakeLaserDevice fakeLaser = new FakeLaserDevice();
+    LaserScanPublisher scanPublisher = new LaserScanPublisher(fakeLaser);
+    NodeConfiguration nodeConfiguration = NodeConfiguration.newPrivate();
+    scanPublisher.setNode(new DefaultNode(nodeConfiguration.setNodeName("android_hokuyo_test_node")));
+    try {
+      scanPublisher.toLaserScanMessage("/base_scan", fakeLaser.makeFakeScan());
+      fail();
+    } catch (IllegalStateException e) {
+      // This should throw because our laser scan has too few range
+      // measurements.
+      // It expects three according to our configuration.
+    }
+  }
+}