Option min_intensity #131

SICKAG · Jul 13, 2021 · 9753e68 · 9753e68
1 parent 583feb8
commit 9753e68
Show file tree

Hide file tree

Showing 32 changed files with 51 additions and 0 deletions.
diff --git a/README.md b/README.md
@@ -196,6 +196,9 @@ The use of the parameters can be looked up in the launch files. This is also rec
 - `intensity_resolution_16bit`
   If true, the intensity values is transferred as 16 bit value. If false, as 8 bit value.
 
+- `min_intensity`
+  If min_intensity > 0, all range values in a LaserScan message are set to infinity, if their intensity value is below min_intensity
+
 - `cloud_topic`
   Topic name of the published pointcloud2 data
 

diff --git a/cfg/SickScan.cfg b/cfg/SickScan.cfg
@@ -57,6 +57,7 @@ gen.add("min_ang",        double_t, 0, "The angle of the first range measurement
 gen.add("max_ang",        double_t, 0, "The angle of the last range measurement [rad].",                            pi,-2*pi,2*pi)
 gen.add("intensity",      bool_t,   0, "Whether or not to return intensity values.    ",                              True)
 gen.add("intensity_resolution_16bit",      bool_t,   0, "True = 16Bit False = 8Bit.    ",                              True)
+gen.add("min_intensity",   double_t, 0, "Set range of LaserScan messages to infinity, if intensity < min_intensity", 0.0, 0.0, 65536.0)
 gen.add("skip",           int_t,    0, "The number of scans to skip between each measured scan.",                     0,          0,         9)
 gen.add("frame_id",       str_t,    0, "The TF frame in which laser scans will be returned.",                        "laser")
 gen.add("imu_frame_id",       str_t,    0, "The TF frame in which imu_data will be returned.",                        "imu_link")

diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp
@@ -337,6 +337,7 @@ namespace sick_scan
   // FIXME All Tims have 15Hz
   {
     expectedFrequency_ = this->parser_->getCurrentParamPtr()->getExpectedFrequency();
+    m_min_intensity = 0.0; // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
 
     setSensorIsRadar(false);
     init_cmdTables();
@@ -1177,6 +1178,8 @@ namespace sick_scan
 
     pn.getParam("intensity", rssiFlag);
     pn.getParam("intensity_resolution_16bit", rssiResolutionIs16Bit);
+    pn.getParam("min_intensity", m_min_intensity); // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
+
     //check new ip adress and add cmds to write ip to comand chain
     std::string sNewIPAddr = "";
     boost::asio::ip::address_v4 ipNewIPAddr;
@@ -3759,6 +3762,18 @@ namespace sick_scan
                       sendMsg = false;
                     }
                   }
+                  // If msg.intensities[j] < min_intensity, then set msg.ranges[j] to inf according to https://github.com/SICKAG/sick_scan/issues/131
+                  if(m_min_intensity > 0) // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
+                  {
+                    for (int j = 0, j_max = (int)std::min(msg.ranges.size(), msg.intensities.size()); j < j_max; j++)
+                    {
+                      if(msg.intensities[j] < m_min_intensity)
+                      {
+                        msg.ranges[j] = std::numeric_limits<float>::infinity();
+                        // ROS_DEBUG_STREAM("msg.intensities[" << j << "]=" << msg.intensities[j] << " < " << m_min_intensity << ", set msg.ranges[" << j << "]=" << msg.ranges[j] << " to infinity.");
+                      }
+                    }
+                  }
 
                 }
 #ifndef _MSC_VER

diff --git a/include/sick_scan/sick_scan_common.h b/include/sick_scan/sick_scan_common.h
@@ -392,6 +392,9 @@ namespace sick_scan
     bool sensorIsRadar;
 
     AngleCompensator *angleCompensator = NULL;
+
+    double m_min_intensity; // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
+
   };
 
 } /* namespace sick_scan */

diff --git a/launch/sick_lms_1xx.launch b/launch/sick_lms_1xx.launch
@@ -19,5 +19,6 @@
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
         <param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_lms_1xx_encoder.launch b/launch/sick_lms_1xx_encoder.launch
@@ -28,5 +28,6 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="encoder_mode" type="int" value="1"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_lms_1xxx.launch b/launch/sick_lms_1xxx.launch
@@ -27,5 +27,6 @@
         <param name="intensity_resolution_16bit" type="bool" value="false"/>
         <param name="min_ang" type="double" value="-2.391"/>
         <param name="max_ang" type="double" value="2.391"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_lms_4xxx.launch b/launch/sick_lms_4xxx.launch
@@ -58,6 +58,7 @@
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 
 </launch>

diff --git a/launch/sick_lms_4xxx_encoder.launch b/launch/sick_lms_4xxx_encoder.launch
@@ -59,6 +59,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="encoder_mode" type="int" value="1"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 
 </launch>

diff --git a/launch/sick_lms_5xx.launch b/launch/sick_lms_5xx.launch
@@ -52,6 +52,7 @@ Check IP-address, if you scanner is not found after roslaunch.
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
         <param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
     </node>
 </launch>

diff --git a/launch/sick_lms_5xx_encoder.launch b/launch/sick_lms_5xx_encoder.launch
@@ -40,6 +40,7 @@ Check IP-address, if you scanner is not found after roslaunch.
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="use_binary_protocol" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
         <param name="encoder_mode" type="int" value="2"/>
         <!-- possible angular resolutions are 1,0.6667,0.5,0.3333,0.25,0.1667
          possible scan frequencies are 25,35,50,75,100 Hz

diff --git a/launch/sick_mrs_1xxx.launch b/launch/sick_mrs_1xxx.launch
@@ -39,6 +39,7 @@ default max_angle for this scanner type is +137.5 degree.
         <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
     </node>
 </launch>

diff --git a/launch/sick_mrs_1xxx_cartographer.launch b/launch/sick_mrs_1xxx_cartographer.launch
@@ -43,6 +43,7 @@ default max_angle for this scanner type is +137.5 degree.
         <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
     </node>
 </launch>

diff --git a/launch/sick_mrs_6xxx.launch b/launch/sick_mrs_6xxx.launch
@@ -31,6 +31,7 @@
         <param name="max_ang" type="double" value="+1.047"/>
         <param name="use_binary_protocol" type="bool" value="True"/>
         <param name="sw_pll_only_publish" type="bool" value="False"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
 
     </node>

diff --git a/launch/sick_mrs_6xxx_first_echo.launch b/launch/sick_mrs_6xxx_first_echo.launch
@@ -28,6 +28,7 @@
         <param name="min_ang" type="double" value="-1.047"/>
         <param name="max_ang" type="double" value="+1.047"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
 
     </node>

diff --git a/launch/sick_mrs_6xxx_last_echo.launch b/launch/sick_mrs_6xxx_last_echo.launch
@@ -27,6 +27,7 @@
         <param name="min_ang" type="double" value="-1.047"/>
         <param name="max_ang" type="double" value="+1.047"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
 
     </node>

diff --git a/launch/sick_mrs_6xxx_narrow.launch b/launch/sick_mrs_6xxx_narrow.launch
@@ -29,6 +29,7 @@
         <param name="min_ang" type="double" value="-0.523598667"/>
         <param name="max_ang" type="double" value="+0.523598667"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
 
     </node>

diff --git a/launch/sick_nav_2xx.launch b/launch/sick_nav_2xx.launch
@@ -12,5 +12,6 @@
         <param name="hostname" type="string" value="$(arg hostname)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_nav_3xx.launch b/launch/sick_nav_3xx.launch
@@ -23,6 +23,7 @@
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 
 </launch>
diff --git a/launch/sick_nav_3xx_ascii.launch b/launch/sick_nav_3xx_ascii.launch
@@ -20,6 +20,7 @@
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 
 </launch>
diff --git a/launch/sick_new_ip.launch b/launch/sick_new_ip.launch
@@ -19,6 +19,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="new_IP_address" type="string" value="$(arg new_IP)"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 
 </launch>

diff --git a/launch/sick_rms_3xx.launch b/launch/sick_rms_3xx.launch
@@ -26,5 +26,6 @@
         <param name="transmit_raw_targets" type="bool" value="true"/>
         <param name="transmit_objects" type="bool" value="true"/>
         <param name="emul_sensor" type="bool" value="$(arg emul_sensor)"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_rms_3xx_emul.launch b/launch/sick_rms_3xx_emul.launch
@@ -15,5 +15,6 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="emul_sensor" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_tim_240.launch b/launch/sick_tim_240.launch
@@ -37,5 +37,6 @@
         <param name="port" type="string" value="$(arg port)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_tim_240_bin.launch b/launch/sick_tim_240_bin.launch
@@ -37,5 +37,6 @@
         <param name="port" type="string" value="$(arg port)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_tim_4xx.launch b/launch/sick_tim_4xx.launch
@@ -61,6 +61,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="false"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
 

diff --git a/launch/sick_tim_5xx.launch b/launch/sick_tim_5xx.launch
@@ -37,6 +37,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
         <!--

diff --git a/launch/sick_tim_5xx_ASCII.launch b/launch/sick_tim_5xx_ASCII.launch
@@ -52,6 +52,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
         <!--

diff --git a/launch/sick_tim_5xx_new_ip.launch b/launch/sick_tim_5xx_new_ip.launch
@@ -30,6 +30,7 @@
     <param name="port" type="string" value="2112" />
     <param name="timelimit" type="int" value="5" />
     <param name="new_IP_address" type="string" value="$(arg new_IP)"/>
+    <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
   </node>
 </launch>
 

diff --git a/launch/sick_tim_5xx_twin.launch b/launch/sick_tim_5xx_twin.launch
@@ -29,6 +29,7 @@
         <param name="hostname" type="string" value="$(arg hostname1)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
         <remap from="scan" to="scan_1"/>
         <remap from="cloud" to="cloud_1"/>
     </node>
@@ -42,6 +43,7 @@
         <param name="hostname" type="string" value="$(arg hostname2)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
         <remap from="scan" to="scan_2"/>
         <remap from="cloud" to="cloud_2"/>
     </node>

diff --git a/launch/sick_tim_7xx.launch b/launch/sick_tim_7xx.launch
@@ -40,6 +40,7 @@
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
         <param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 
 </launch>

diff --git a/launch/sick_tim_7xxS.launch b/launch/sick_tim_7xxS.launch
@@ -40,6 +40,7 @@
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
         <param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
+        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
         <!--