Added inf to be set for min range

clearpathrobotics · Nov 24, 2017 · cb81bb0 · cb81bb0
1 parent dec2cdb
commit cb81bb0
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 1 deletion.
diff --git a/launch/LMS1xx.launch b/launch/LMS1xx.launch
@@ -1,6 +1,8 @@
 <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">
     <param name="host" value="$(arg host)" />
+    <param name="publish_min_range_as_inf" value="$(arg publish_min_range_as_inf)" />
   </node>
 </launch>
diff --git a/src/LMS1xx_node.cpp b/src/LMS1xx_node.cpp
@@ -41,6 +41,7 @@ int main(int argc, char **argv)
   // parameters
   std::string host;
   std::string frame_id;
+  bool inf_range;
   int port;
 
   ros::init(argc, argv, "lms1xx");
@@ -50,6 +51,7 @@ int main(int argc, char **argv)
 
   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<int>("port", port, 2111);
 
   while (ros::ok())
@@ -178,7 +180,14 @@ int main(int argc, char **argv)
       {
         for (int i = 0; i < data.dist_len1; i++)
         {
-          scan_msg.ranges[i] = data.dist1[i] * 0.001;
+          float range_data = data.dist1[i] * 0.001;
+
+          if(inf_range && range_data < scan_msg.range_min){
+            scan_msg.ranges[i] = std::numeric_limits<float>::infinity();
+          }
+          else{
+            scan_msg.ranges[i] = range_data;
+          }
         }
 
         for (int i = 0; i < data.rssi_len1; i++)