Update the depthtolaserscan

The launch file now demonstrates how to change the default parameters.

Also tweaked the subscriber to record a timestamp first thing, so the
scan data is associated with the depth image arrival time (not the
time of its publication).

depthtolaserscan/launch/laserscan.launch

@@ -58,7 +58,12 @@
   <node name="depthtolaserscan"
 	pkg="depthtolaserscan"
   	type="depthtolaserscan"
-	output="screen"/>
+	output="screen">
+    <param name="min_height"   value="0.030"/>
+    <param name="max_height"   value="0.250"/>
+    <param name="horzsamples"  value="-1"/>
+    <!-- (-1) = default = half of image width = 424 -->
+  </node>
 
   <!-- Publish all the robot frames. -->
   <node name="robot_state_publisher"

depthtolaserscan/scripts/depthtolaserscan.py

@@ -308,6 +308,13 @@ def setSampleList():
 #   Convert the depth image into a laser scan
 #
 def callback_image(imagemsg):
+    # Record the arrival time.  The imagemsg timestamp *should* tell
+    # us when the underlying data was sampled, but the RealSense
+    # timestamps are incorrect.  So this is our best guess.
+    # Potentially we could shift a small delay back in time, as we
+    # know it takes finite time to process the data and get it here.
+    timestamp = rospy.Time.now()
+
     # Report (for debugging).
     rospy.loginfo("Image #%4d at %10d secs"
                   % (imagemsg.header.seq, imagemsg.header.stamp.secs))
@@ -352,7 +359,7 @@ def callback_image(imagemsg):
 
     # Create scan msg.
     scanmsg = LaserScan()
-    scanmsg.header.stamp    = rospy.Time.now()  # imagemsg.header.stamp
+    scanmsg.header.stamp    = timestamp
     scanmsg.header.frame_id = laserframe
     scanmsg.angle_min       = angle_min
     scanmsg.angle_max       = angle_max

depthtolaserscan/src/depthtolaserscan.cpp

@@ -40,7 +40,7 @@ using namespace sensor_msgs;
 /*
 **   Constants
 */
-#define TIMEOUT     (5.0)	// Timeout for the initialization
+#define TIMEOUT     (10.0)	// Timeout for the initialization
 
 #define INFO_TOPIC  "/camera/depth/camera_info"
 #define IMAGE_TOPIC "/camera/depth/image_rect_raw"
@@ -509,6 +509,13 @@ int prepareSampling()
 */
 void callback_image(const Image::ConstPtr& imagemsgp)
 {
+  // Record the arrival time.  The imagemsg timestamp *should* tell us
+  // when the underlying data was sampled, but the RealSense
+  // timestamps are incorrect.  So this is our best guess.
+  // Potentially we could shift a small delay back in time, as we know
+  // it takes finite time to process the data and get it here.
+  Time timestamp = Time::now();
+
   // Report (for debugging).
   // ROS_INFO("Image #%4d at %10d secs",
   //          imagemsgp->header.seq, imagemsgp->header.stamp.sec);
@@ -573,7 +580,7 @@ void callback_image(const Image::ConstPtr& imagemsgp)
     }
 
   // Update the mssage and publish.  Note the depth camera time is bad!
-  scanmsg.header.stamp = Time::now();
+  scanmsg.header.stamp = timestamp;
   scanpub.publish(scanmsg);
 }