diff --git a/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/LaserScanPublisher.java b/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/LaserScanPublisher.java
index f96077e..92525ff 100644
--- a/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/LaserScanPublisher.java
+++ b/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/LaserScanPublisher.java
@@ -105,11 +105,16 @@ sensor_msgs.LaserScan toLaserScanMessage(String laserFrame, LaserScan scan,
         "Number of scans in configuration does not match received range measurements (%d > %d).",
         numberOfConfiguredRanges, scan.getRanges().length));
     float[] ranges = new float[numberOfConfiguredRanges];
+    int scannedRanges = scan.getRanges().length;
     for (int i = 0; i < numberOfConfiguredRanges; i++) {
       int step = i + configuration.getFirstStep();
       // Select only the configured range measurements and convert from
       // millimeters to meters.
-      ranges[i] = (float) (scan.getRanges()[step] / 1000.0);
+      if(step>=scannedRanges) {
+        ranges[i] = 0;
+      } else {
+        ranges[i] = (float) (scan.getRanges()[step] / 1000.0);
+      }
     }
     result.setRanges(ranges);
     result.setTimeIncrement(configuration.getTimeIncrement());
diff --git a/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/scip20/Device.java b/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/scip20/Device.java
index b2b0524..04dda27 100644
--- a/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/scip20/Device.java
+++ b/hokuyo/src/main/java/com/github/rosjava/rosjava_extras/hokuyo/scip20/Device.java
@@ -117,6 +117,8 @@ private void waitUntilReady() {
     boolean ready = false;
     while (!ready) {
       ready = true;
+      // NOTE: At this moment, the sent command does not matter much.
+      // Currently fixed to (0; 768)
       write("MD0000076800001");
       try {
         checkMdmsStatus();
@@ -260,8 +262,9 @@ public void startScanning(final LaserScanListener listener) {
       @Override
       public void run() {
         while (true) {
-          String command = "MD00000768000%02d";
-          write(String.format(command, 99));
+          String command = String.format("MD%04d%04d000%02d", configuration.getFirstStep(),
+                  configuration.getLastStep(), 99);
+          write(command);
           checkMdmsStatus();
           checkTerminator();
           String scansRemaining = "99";