Skip to content
This repository was archived by the owner on Mar 8, 2023. It is now read-only.

Commit 9753e68

Browse files
rostestrostest
committed
Option min_intensity #131 
1 parent 583feb8 commit 9753e68

32 files changed

+51
-0
lines changed

‎README.md‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -196,6 +196,9 @@ The use of the parameters can be looked up in the launch files. This is also rec
196196
- `intensity_resolution_16bit`
197197
If true, the intensity values is transferred as 16 bit value. If false, as 8 bit value.
198198

199+
- `min_intensity`
200+
If min_intensity > 0, all range values in a LaserScan message are set to infinity, if their intensity value is below min_intensity
201+
199202
- `cloud_topic`
200203
Topic name of the published pointcloud2 data
201204

‎cfg/SickScan.cfg‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@ gen.add("min_ang", double_t, 0, "The angle of the first range measurement
5757
gen.add("max_ang", double_t, 0, "The angle of the last range measurement [rad].", pi,-2*pi,2*pi)
5858
gen.add("intensity", bool_t, 0, "Whether or not to return intensity values. ", True)
5959
gen.add("intensity_resolution_16bit", bool_t, 0, "True = 16Bit False = 8Bit. ", True)
60+
gen.add("min_intensity", double_t, 0, "Set range of LaserScan messages to infinity, if intensity < min_intensity", 0.0, 0.0, 65536.0)
6061
gen.add("skip", int_t, 0, "The number of scans to skip between each measured scan.", 0, 0, 9)
6162
gen.add("frame_id", str_t, 0, "The TF frame in which laser scans will be returned.", "laser")
6263
gen.add("imu_frame_id", str_t, 0, "The TF frame in which imu_data will be returned.", "imu_link")

‎driver/src/sick_scan_common.cpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -337,6 +337,7 @@ namespace sick_scan
337337
// FIXME All Tims have 15Hz
338338
{
339339
expectedFrequency_ = this->parser_->getCurrentParamPtr()->getExpectedFrequency();
340+
m_min_intensity = 0.0; // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
340341

341342
setSensorIsRadar(false);
342343
init_cmdTables();
@@ -1177,6 +1178,8 @@ namespace sick_scan
11771178

11781179
pn.getParam("intensity", rssiFlag);
11791180
pn.getParam("intensity_resolution_16bit", rssiResolutionIs16Bit);
1181+
pn.getParam("min_intensity", m_min_intensity); // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
1182+
11801183
//check new ip adress and add cmds to write ip to comand chain
11811184
std::string sNewIPAddr = "";
11821185
boost::asio::ip::address_v4 ipNewIPAddr;
@@ -3759,6 +3762,18 @@ namespace sick_scan
37593762
sendMsg = false;
37603763
}
37613764
}
3765+
// If msg.intensities[j] < min_intensity, then set msg.ranges[j] to inf according to https://github.com/SICKAG/sick_scan/issues/131
3766+
if(m_min_intensity > 0) // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
3767+
{
3768+
for (int j = 0, j_max = (int)std::min(msg.ranges.size(), msg.intensities.size()); j < j_max; j++)
3769+
{
3770+
if(msg.intensities[j] < m_min_intensity)
3771+
{
3772+
msg.ranges[j] = std::numeric_limits<float>::infinity();
3773+
// ROS_DEBUG_STREAM("msg.intensities[" << j << "]=" << msg.intensities[j] << " < " << m_min_intensity << ", set msg.ranges[" << j << "]=" << msg.ranges[j] << " to infinity.");
3774+
}
3775+
}
3776+
}
37623777

37633778
}
37643779
#ifndef _MSC_VER

‎include/sick_scan/sick_scan_common.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -392,6 +392,9 @@ namespace sick_scan
392392
bool sensorIsRadar;
393393

394394
AngleCompensator *angleCompensator = NULL;
395+
396+
double m_min_intensity; // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
397+
395398
};
396399

397400
} /* namespace sick_scan */

‎launch/sick_lms_1xx.launch‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,5 +19,6 @@
1919
<param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
2020
<param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
2121
<param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
22+
<param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
2223
</node>
2324
</launch>

‎launch/sick_lms_1xx_encoder.launch‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,5 +28,6 @@
2828
<param name="port" type="string" value="2112"/>
2929
<param name="timelimit" type="int" value="5"/>
3030
<param name="encoder_mode" type="int" value="1"/>
31+
<param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
3132
</node>
3233
</launch>

‎launch/sick_lms_1xxx.launch‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,5 +27,6 @@
2727
<param name="intensity_resolution_16bit" type="bool" value="false"/>
2828
<param name="min_ang" type="double" value="-2.391"/>
2929
<param name="max_ang" type="double" value="2.391"/>
30+
<param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
3031
</node>
3132
</launch>

‎launch/sick_lms_4xxx.launch‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@
5858
<param name="frame_id" type="str" value="$(arg frame_id)"/>
5959
<param name="port" type="string" value="2112"/>
6060
<param name="timelimit" type="int" value="5"/>
61+
<param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
6162
</node>
6263

6364
</launch>

‎launch/sick_lms_4xxx_encoder.launch‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@
5959
<param name="port" type="string" value="2112"/>
6060
<param name="timelimit" type="int" value="5"/>
6161
<param name="encoder_mode" type="int" value="1"/>
62+
<param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
6263
</node>
6364

6465
</launch>

‎launch/sick_lms_5xx.launch‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ Check IP-address, if you scanner is not found after roslaunch.
5252
<param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
5353
<param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
5454
<param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
55+
<param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
5556

5657
</node>
5758
</launch>

0 commit comments

Comments
 (0)