diff --git a/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp index 73c57e1d6..a5c62e078 100644 --- a/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp @@ -76,6 +76,9 @@ class GazeboRosRaySensorPrivate /// brief Radiation type to report when output type is range uint8_t range_radiation_type_; + /// If set, the range sensor becomes a proximity sensor and this its threshold. + double proximity_threshold_{0.0}; + /// Gazebo node used to subscribe to laser scan gazebo::transport::NodePtr gazebo_node_; @@ -156,6 +159,10 @@ void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt _sdf->Get("radiation_type").c_str()); return; } + if (_sdf->HasElement("proximity_threshold")) { + impl_->proximity_threshold_ = _sdf->Get("proximity_threshold"); + } + } if (!_sdf->HasElement("min_intensity")) { @@ -232,6 +239,13 @@ void GazeboRosRaySensorPrivate::PublishRange(ConstLaserScanStampedPtr & _msg) range_msg.header.frame_id = frame_name_; // Set radiation type from sdf range_msg.radiation_type = range_radiation_type_; + + if (proximity_threshold_ != 0.0) { + range_msg.min_range = range_msg.max_range = proximity_threshold_; + range_msg.range = range_msg.range < + proximity_threshold_ ? -std::numeric_limits::infinity() : + std::numeric_limits::infinity(); + } // Publish output boost::get(pub_)->publish(range_msg); }