Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add Proximity sensor option #1424

Open
wants to merge 1 commit into
base: ros2
Choose a base branch
from
Open

Add Proximity sensor option #1424

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions gazebo_plugins/src/gazebo_ros_ray_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand Down Expand Up @@ -156,6 +159,10 @@ void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt
_sdf->Get<std::string>("radiation_type").c_str());
return;
}
if (_sdf->HasElement("proximity_threshold")) {
impl_->proximity_threshold_ = _sdf->Get<double>("proximity_threshold");
}

}

if (!_sdf->HasElement("min_intensity")) {
Expand Down Expand Up @@ -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<sensor_msgs::msg::Range::_range_type>::infinity() :
std::numeric_limits<sensor_msgs::msg::Range::_range_type>::infinity();
}
// Publish output
boost::get<RangePub>(pub_)->publish(range_msg);
}
Expand Down