Implemented a nodelet version of the driver.

CMakeLists.txt

@@ -5,18 +5,21 @@ project(lms1xx)
 find_package(console_bridge REQUIRED)
 include_directories(include ${console_bridge_INCLUDE_DIRS})
 add_library(LMS1xx src/LMS1xx.cpp)
+set_property(TARGET LMS1xx PROPERTY POSITION_INDEPENDENT_CODE ON)
 target_link_libraries(LMS1xx ${console_bridge_LIBRARIES})
 
 # Regular catkin package follows.
-find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs)
-catkin_package(CATKIN_DEPENDS roscpp)
+find_package(catkin REQUIRED COMPONENTS nodelet roscpp sensor_msgs)
+catkin_package(CATKIN_DEPENDS nodelet roscpp)
 
 include_directories(include ${catkin_INCLUDE_DIRS})
 add_executable(LMS1xx_node src/LMS1xx_node.cpp)
 target_link_libraries(LMS1xx_node LMS1xx ${catkin_LIBRARIES})
+add_library(LMS1xx_nodelet src/LMS1xx_nodelet.cpp)
+target_link_libraries(LMS1xx_nodelet LMS1xx ${catkin_LIBRARIES})
 
 
-install(TARGETS LMS1xx LMS1xx_node
+install(TARGETS LMS1xx LMS1xx_node LMS1xx_nodelet
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -25,6 +28,9 @@ install(TARGETS LMS1xx LMS1xx_node
 install(DIRECTORY meshes launch urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
+install(FILES nodelets.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
 install(PROGRAMS scripts/find_sick scripts/set_sick_ip
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 

README.md

@@ -2,3 +2,10 @@ LMS1xx [![Build Status](https://travis-ci.org/clearpathrobotics/LMS1xx.svg?branc
 ======
 
 ROS driver for the SICK LMS1xx family of laser scanners. Originally from [RCPRG](https://github.com/RCPRG-ros-pkg/RCPRG_laser_drivers).
+
+
+Docs
+====
+
+* [Technical Information](https://cdn.sick.com/media/docs/7/27/927/Technical_information_Telegram_Listing_Ranging_sensors_LMS1xx_LMS5xx_TiM5xx_TiM7xx_LMS1000_MRS1000_MRS6000_NAV310_LD_OEM15xx_LD_LRS36xx_LMS4000_en_IM0045927.PDF)
+* [Operating Instructions](https://cdn.sick.com/media/docs/1/31/331/Operating_instructions_LMS1xx_Laser_Measurement_Sensors_en_IM0031331.PDF)

launch/LMS1xx.launch

@@ -1,8 +1,23 @@
 <launch>
   <arg name="host" default="192.168.1.14" />
   <arg name="publish_min_range_as_inf" default="false" />
-  <node pkg="lms1xx" name="lms1xx" type="LMS1xx_node">
+  <arg name="nodelet_manager" default="" />
+  <arg name="publish_2nd_pulse" default="false" />
+  <arg name="range_min" default="0.01" />
+  <arg name="range_max" default="20.0" />
+
+  <node pkg="lms1xx" name="lms1xx" type="LMS1xx_node" if="$(eval nodelet_manager=='')">
     <param name="host" value="$(arg host)" />
     <param name="publish_min_range_as_inf" value="$(arg publish_min_range_as_inf)" />
+    <param name="publish_2nd_pulse" value="$(arg publish_2nd_pulse)" />
+    <param name="range_min" value="$(arg range_min)" />
+    <param name="range_max" value="$(arg range_max)" />
+  </node>
+  <node pkg="nodelet" name="lms1xx" type="nodelet" args="load lms1xx/LMS1xx_nodelet $(arg nodelet_manager)" if="$(eval nodelet_manager!='')">
+    <param name="host" value="$(arg host)" />
+    <param name="publish_min_range_as_inf" value="$(arg publish_min_range_as_inf)" />
+    <param name="publish_2nd_pulse" value="$(arg publish_2nd_pulse)" />
+    <param name="range_min" value="$(arg range_min)" />
+    <param name="range_max" value="$(arg range_max)" />
   </node>
 </launch>

nodelets.xml

@@ -0,0 +1,7 @@
+<library path="lib/libLMS1xx_nodelet">
+  <class name="lms1xx/LMS1xx_nodelet" type="lms1xx::LMS1xxNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet version of LMS1xx driver.
+    </description>
+  </class>
+</library>

package.xml

@@ -11,6 +11,7 @@
   <license>LGPL</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>nodelet</depend>
   <depend>rosconsole_bridge</depend>
   <depend>roscpp</depend>
   <depend>roscpp_serialization</depend>
@@ -19,4 +20,8 @@
   <test_depend>roslaunch</test_depend>
   <test_depend>roslint</test_depend>
   <test_depend>rosunit</test_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelets.xml" />
+  </export>
 </package>

src/LMS1xx.cpp

@@ -363,6 +363,27 @@ void LMS1xx::parseScanData(char* buffer, scanData* data)
       data->rssi_len2 = NumberData;
     }
 
+
+    /*
+     * TODO: there are range/rssi values with special meaning!
+     *
+     * LMDscandata - reserved valuesValid distance measurement values are values starting from 16d  upwards;
+     * everything below  has the  following meaning:
+     *
+     * DIST  RSSI  Description
+     * 0d    0h    no meas value detected; means that in the angle, there was no valid measurement
+     *             value. Probably the object to measure was out of the range of the or the object
+     *             was reflecting too less light back (black objects)
+     * 1d    FFFFh (16Bit output)
+     *       FFh(8Bit output)
+     *             dazzled, blinded
+     * 2d    0h    implausible measurement values
+     * 3d    0h    value was set to invalid by a filter (Echo Filter,Particle Filterin old firmware)
+     * 4–15d 0h    reserved, at the moment not given out, if there occurs a value in that range any
+     *             way perform a Softwareupdate
+     * ≥16d  >0h   valid measurement val
+     */
+
     for (int i = 0; i < NumberData; i++)
     {
       int dat;

src/LMS1xx_node.cpp

@@ -38,22 +38,28 @@ int main(int argc, char **argv)
   scanCfg cfg;
   scanOutputRange outputRange;
   scanDataCfg dataCfg;
-  sensor_msgs::LaserScan scan_msg;
+  sensor_msgs::LaserScan scan_msg, scan2_msg;
 
   // parameters
   std::string host;
   std::string frame_id;
   bool inf_range;
+  bool publish_2nd_pulse;
   int port;
+  float range_min, range_max;
 
   ros::init(argc, argv, "lms1xx");
   ros::NodeHandle nh;
   ros::NodeHandle n("~");
   ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
+  ros::Publisher scan2_pub = nh.advertise<sensor_msgs::LaserScan>("scan_2nd_pulse", 1);
 
   n.param<std::string>("host", host, "192.168.1.2");
   n.param<std::string>("frame_id", frame_id, "laser");
   n.param<bool>("publish_min_range_as_inf", inf_range, false);
+  n.param<bool>("publish_2nd_pulse", publish_2nd_pulse, false);
+  n.param<float>("range_min", range_min, 0.01);
+  n.param<float>("range_max", range_max, 20.0);
   n.param<int>("port", port, 2111);
 
   while (ros::ok())
@@ -87,14 +93,16 @@ int main(int argc, char **argv)
     ROS_DEBUG("Laser output range:angleResolution %d, startAngle %d, stopAngle %d",
               outputRange.angleResolution, outputRange.startAngle, outputRange.stopAngle);
 
-    scan_msg.header.frame_id = frame_id;
-    scan_msg.range_min = 0.01;
-    scan_msg.range_max = 20.0;
-    scan_msg.scan_time = 100.0 / cfg.scaningFrequency;
-    scan_msg.angle_increment = static_cast<double>(outputRange.angleResolution / 10000.0 * DEG2RAD);
-    scan_msg.angle_min = static_cast<double>(outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2);
-    scan_msg.angle_max = static_cast<double>(outputRange.stopAngle / 10000.0 * DEG2RAD - M_PI / 2);
-
+    scan_msg.header.frame_id = scan2_msg.header.frame_id = frame_id;
+    scan_msg.range_min = scan2_msg.range_min = range_min;
+    scan_msg.range_max = scan2_msg.range_max = range_max;
+    scan_msg.scan_time = scan2_msg.scan_time = 100.0 / cfg.scaningFrequency;
+    scan_msg.angle_increment = scan2_msg.angle_increment =
+        static_cast<double>(outputRange.angleResolution / 10000.0 * DEG2RAD);
+    scan_msg.angle_min = scan2_msg.angle_min = static_cast<double>(
+        outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2);
+    scan_msg.angle_max = scan2_msg.angle_max = static_cast<double>(
+        outputRange.stopAngle / 10000.0 * DEG2RAD - M_PI / 2);
     ROS_DEBUG_STREAM("Device resolution is " << (double)outputRange.angleResolution / 10000.0 << " degrees.");
     ROS_DEBUG_STREAM("Device frequency is " << (double)cfg.scaningFrequency / 100.0 << " Hz");
 
@@ -107,15 +115,19 @@ int main(int argc, char **argv)
     }
     scan_msg.ranges.resize(num_values);
     scan_msg.intensities.resize(num_values);
+    if (publish_2nd_pulse) {
+      scan2_msg.ranges.resize(num_values);
+      scan2_msg.intensities.resize(num_values);
+    }
 
-    scan_msg.time_increment =
+    scan_msg.time_increment = scan2_msg.time_increment =
       (outputRange.angleResolution / 10000.0)
       / 360.0
       / (cfg.scaningFrequency / 100.0);
 
     ROS_DEBUG_STREAM("Time increment is " << static_cast<int>(scan_msg.time_increment * 1000000) << " microseconds");
 
-    dataCfg.outputChannel = 1;
+    dataCfg.outputChannel = publish_2nd_pulse ? 3 : 1;
     dataCfg.remission = true;
     dataCfg.resolution = 1;
     dataCfg.encoder = 0;
@@ -173,16 +185,17 @@ int main(int argc, char **argv)
     {
       ros::Time start = ros::Time::now();
 
-      scan_msg.header.stamp = start;
+      scan_msg.header.stamp = scan2_msg.header.stamp = start;
       ++scan_msg.header.seq;
+      ++scan2_msg.header.seq;
 
       scanData data;
       ROS_DEBUG("Reading scan data.");
       if (laser.getScanData(&data))
       {
         for (int i = 0; i < data.dist_len1; i++)
         {
-          float range_data = data.dist1[i] * 0.001;
+          float range_data = data.dist1[i] * 0.001f;
 
           if (inf_range && range_data < scan_msg.range_min)
           {
@@ -201,6 +214,25 @@ int main(int argc, char **argv)
 
         ROS_DEBUG("Publishing scan data.");
         scan_pub.publish(scan_msg);
+
+        if (publish_2nd_pulse) {
+          for (int i = 0; i < data.dist_len2; i++) {
+            float range_data = data.dist2[i] * 0.001f;
+
+            if (inf_range && range_data < scan2_msg.range_min) {
+              scan2_msg.ranges[i] = std::numeric_limits<float>::infinity();
+            } else {
+              scan2_msg.ranges[i] = range_data;
+            }
+          }
+
+          for (int i = 0; i < data.rssi_len2; i++) {
+            scan2_msg.intensities[i] = data.rssi2[i];
+          }
+
+          ROS_DEBUG("Publishing second pulse scan data.");
+          scan2_pub.publish(scan2_msg);
+        }
       }
       else
       {