added modified range plugin that sends nan when > max_range

ros-simulation · Aug 6, 2021 · a2defb4 · a2defb4
1 parent 231a721
commit a2defb4
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 5 deletions.
diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt
@@ -153,7 +153,7 @@ catkin_package(
   gazebo_ros_skid_steer_drive
   gazebo_ros_video
   gazebo_ros_planar_move
-  gazebo_ros_range
+  gazebo_ros_range_iq
   gazebo_ros_vacuum_gripper
 
   CATKIN_DEPENDS
@@ -326,8 +326,8 @@ target_link_libraries(gazebo_ros_hand_of_god ${catkin_LIBRARIES} ${Boost_LIBRARI
 add_library(gazebo_ros_ft_sensor src/gazebo_ros_ft_sensor.cpp)
 target_link_libraries(gazebo_ros_ft_sensor ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
-add_library(gazebo_ros_range src/gazebo_ros_range.cpp)
-target_link_libraries(gazebo_ros_range ${catkin_LIBRARIES} ${Boost_LIBRARIES} RayPlugin)
+add_library(gazebo_ros_range_iq src/gazebo_ros_range.cpp)
+target_link_libraries(gazebo_ros_range_iq ${catkin_LIBRARIES} ${Boost_LIBRARIES} RayPlugin)
 
 add_library(gazebo_ros_vacuum_gripper src/gazebo_ros_vacuum_gripper.cpp)
 target_link_libraries(gazebo_ros_vacuum_gripper ${catkin_LIBRARIES} ${Boost_LIBRARIES})
@@ -379,7 +379,7 @@ install(TARGETS
   gazebo_ros_planar_move
   gazebo_ros_vacuum_gripper
   gazebo_ros_gpu_laser
-  gazebo_ros_range
+  gazebo_ros_range_iq
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}

diff --git a/gazebo_plugins/src/gazebo_ros_range.cpp b/gazebo_plugins/src/gazebo_ros_range.cpp
@@ -316,7 +316,10 @@ void GazeboRosRange::PutRangeData(common::Time &_updateTime)
         range_msg_.range = std::min(range_msg_.range + this->GaussianKernel(0,gaussian_noise_), parent_ray_sensor_->RangeMax());
 
     this->parent_ray_sensor_->SetActive(true);
-
+    if (range_msg_.range >= range_msg_.max_range)
+    {
+        range_msg_.range = std::numeric_limits<double>::quiet_NaN();
+    }
     // send data out via ros message
     if (this->range_connect_count_ > 0 && this->topic_name_ != "")
         this->pub_.publish(this->range_msg_);