From 0cd7cadfc006eeeb6a0e594e96102cb2e0a7fb18 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Wed, 1 Aug 2018 14:43:10 -0700 Subject: [PATCH] Simplify ray_sensor using gazebo_ros conversions --- gazebo_plugins/src/gazebo_ros_ray_sensor.cpp | 285 +++--------------- .../test/worlds/gazebo_ros_ray_sensor.world | 18 +- 2 files changed, 46 insertions(+), 257 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp index 9c7642413..70db1b10c 100644 --- a/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp @@ -14,8 +14,8 @@ #include #include -#include #include +#include #include #include @@ -31,9 +31,6 @@ class GazeboRosRaySensorPrivate /// Node for ROS communication. gazebo_ros::Node::SharedPtr rosnode_; - /// Sensor this plugin is attached to - boost::variant sensor_; - // Aliases using LaserScan = sensor_msgs::msg::LaserScan; using PointCloud = sensor_msgs::msg::PointCloud; @@ -51,10 +48,6 @@ class GazeboRosRaySensorPrivate /// TF frame output is published in std::string frame_name_; - /// Store sensor constants like min/max angle, templated for either RaySensor or GpuRaySensor - template - void SetParams(T sensor); - /// Subscribe to gazebo's laserscan, calling the appropriate callback based on output type void SubscribeGazeboLaserScan(); @@ -80,19 +73,6 @@ class GazeboRosRaySensorPrivate gazebo::transport::NodePtr gazebo_node_; /// Gazebo subscribe to parent sensor's laser scan gazebo::transport::SubscriberPtr laser_scan_sub_; - - int rayCount; - int rangeCount; - int verticalRayCount; - int verticalRangeCount; - double minAngle; - double maxAngle; - double verticalMinAngle; - double verticalMaxAngle; - double yDiff; - double pDiff; - double rangeMin; - double rangeMax; }; GazeboRosRaySensor::GazeboRosRaySensor() @@ -104,55 +84,26 @@ GazeboRosRaySensor::~GazeboRosRaySensor() { } -void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) { - using gazebo::sensors::RaySensor; - using gazebo::sensors::RaySensorPtr; - using gazebo::sensors::GpuRaySensor; - using gazebo::sensors::GpuRaySensorPtr; - // Create rosnode configured from sdf impl_->rosnode_ = gazebo_ros::Node::Create("gazebo_ray_sensor", _sdf); - // Set constants from either RaySensor or GpuRaySensor - if (_parent->Type() == "ray") { - auto ray_sensor = std::dynamic_pointer_cast(_parent); - if (!ray_sensor) { - RCLCPP_ERROR(impl_->rosnode_->get_logger(), "Could not cast to ray sensor. Exiting."); - return; - } - impl_->SetParams(ray_sensor); - impl_->sensor_ = ray_sensor; - } else if (_parent->Type() == "gpu_ray") { - auto gpu_sensor = std::dynamic_pointer_cast(_parent); - if (!gpu_sensor) { - RCLCPP_ERROR(impl_->rosnode_->get_logger(), "Could not cast to gpu_ray sensor. Exiting."); - return; - } - impl_->SetParams(gpu_sensor); - impl_->sensor_ = gpu_sensor; - } else { - RCLCPP_ERROR( - impl_->rosnode_->get_logger(), - "Plugin attached to a sensor that is neither ray or gpu_ray. Exiting."); - return; - } - // Get tf frame form sdf if provided - if (!_sdf->HasElement("frameName")) { + if (!_sdf->HasElement("frame_name")) { // Frame defaults to link of parent - impl_->frame_name_ = gazebo_ros::ScopedNameBase(_parent->ParentName()); + impl_->frame_name_ = gazebo_ros::ScopedNameBase(_sensor->ParentName()); } else { - impl_->frame_name_ = _sdf->Get("frameName"); + impl_->frame_name_ = _sdf->Get("frame_name"); } // Get output type from sdf if provided - if (!_sdf->HasElement("outputType")) { + if (!_sdf->HasElement("output_type")) { RCLCPP_WARN( - impl_->rosnode_->get_logger(), "missing , defaults to sensor_msgs/PointCloud2"); + impl_->rosnode_->get_logger(), "missing , defaults to sensor_msgs/PointCloud2"); impl_->pub_ = impl_->rosnode_->create_publisher("~/out"); } else { - std::string output_type_string = _sdf->Get("outputType"); + std::string output_type_string = _sdf->Get("output_type"); if (output_type_string == "sensor_msgs/LaserScan") { impl_->pub_ = impl_->rosnode_->create_publisher("~/out"); } else if (output_type_string == "sensor_msgs/PointCloud") { @@ -162,62 +113,45 @@ void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _parent, sdf::ElementPt } else if (output_type_string == "sensor_msgs/Range") { impl_->pub_ = impl_->rosnode_->create_publisher("~/out"); } else { - RCLCPP_ERROR(impl_->rosnode_->get_logger(), "Invalid [%s]", output_type_string); + RCLCPP_ERROR(impl_->rosnode_->get_logger(), "Invalid [%s]", output_type_string); return; } } // Get parameters specific to Range output from sdf if (impl_->pub_.type() == typeid(GazeboRosRaySensorPrivate::RangePub)) { - if (!_sdf->HasElement("radiationType")) { - RCLCPP_INFO(impl_->rosnode_->get_logger(), "missing , defaulting to infared"); + if (!_sdf->HasElement("radiation_type")) { + RCLCPP_INFO(impl_->rosnode_->get_logger(), "missing , defaulting to infared"); impl_->range_radiation_type_ = sensor_msgs::msg::Range::INFRARED; - } else if ("ultrasound" == _sdf->Get("radiationType")) { + } else if ("ultrasound" == _sdf->Get("radiation_type")) { impl_->range_radiation_type_ = sensor_msgs::msg::Range::ULTRASOUND; - } else if ("infared" == _sdf->Get("radiationType")) { + } else if ("infared" == _sdf->Get("radiation_type")) { impl_->range_radiation_type_ = sensor_msgs::msg::Range::INFRARED; } else { RCLCPP_ERROR( - impl_->rosnode_->get_logger(), "Invalid [%s]. Can be ultrasound or infared", - _sdf->Get("radiationType").c_str()); + impl_->rosnode_->get_logger(), + "Invalid [%s]. Can be ultrasound or infared", + _sdf->Get("radiation_type").c_str()); return; } } - if (!_sdf->HasElement("minIntensity")) { + if (!_sdf->HasElement("min_intensity")) { impl_->min_intensity_ = 0.0; - RCLCPP_DEBUG(impl_->rosnode_->get_logger(), "missing , defaults to %f", + RCLCPP_DEBUG(impl_->rosnode_->get_logger(), "missing , defaults to %f", impl_->min_intensity_); } else { - impl_->min_intensity_ = _sdf->Get("minIntensity"); + impl_->min_intensity_ = _sdf->Get("min_intensity"); } - // Create gazebo tranport node and subscribe to sensor's laser scan impl_->gazebo_node_ = boost::make_shared(); - impl_->gazebo_node_->Init(_parent->WorldName()); + impl_->gazebo_node_->Init(_sensor->WorldName()); // TODO(ironmig): use lazy publisher to only process laser data when output has a subscriber + impl_->sensor_topic_ = _sensor->Topic(); impl_->SubscribeGazeboLaserScan(); } -template -void GazeboRosRaySensorPrivate::SetParams(T sensor) -{ - minAngle = sensor->AngleMin().Radian(); - maxAngle = sensor->AngleMax().Radian(); - verticalMaxAngle = sensor->VerticalAngleMax().Radian(); - verticalMinAngle = sensor->VerticalAngleMin().Radian(); - rayCount = sensor->RayCount(); - rangeCount = sensor->RangeCount(); - verticalRayCount = sensor->VerticalRayCount(); - verticalRangeCount = sensor->VerticalRangeCount(); - yDiff = maxAngle - minAngle; - pDiff = verticalMaxAngle - verticalMinAngle; - rangeMin = sensor->RangeMin(); - rangeMax = sensor->RangeMax(); - sensor_topic_ = sensor->Topic(); -} - void GazeboRosRaySensorPrivate::SubscribeGazeboLaserScan() { if (pub_.type() == typeid(LaserScanPub)) { @@ -239,186 +173,43 @@ void GazeboRosRaySensorPrivate::SubscribeGazeboLaserScan() void GazeboRosRaySensorPrivate::PublishLaserScan(ConstLaserScanStampedPtr & _msg) { - sensor_msgs::msg::LaserScan ls; + // Convert Laser scan to ROS LaserScan + auto ls = gazebo_ros::Convert(*_msg); + // Set tf frame ls.header.frame_id = frame_name_; - ls.header.stamp.sec = _msg->time().sec(); - ls.header.stamp.nanosec = _msg->time().nsec(); - ls.angle_min = minAngle; - ls.angle_max = maxAngle; - ls.angle_increment = _msg->scan().angle_step(); - ls.time_increment = 0; - ls.scan_time = 0; - ls.range_min = _msg->scan().range_min(); - ls.range_max = _msg->scan().range_max(); - - // If there are multiple vertical beams, use the one in the middle - size_t start = (verticalRangeCount / 2) * rangeCount; - - // Copy ranges and intensities over - ls.ranges.resize(rangeCount); - ls.intensities.resize(rangeCount); - std::copy(_msg->scan().ranges().begin() + start, - _msg->scan().ranges().begin() + start + rangeCount, - ls.ranges.begin()); - std::copy(_msg->scan().intensities().begin() + start, - _msg->scan().intensities().begin() + start + rangeCount, - ls.intensities.begin()); - + // Publish output boost::get(pub_)->publish(ls); } void GazeboRosRaySensorPrivate::PublishPointCloud(ConstLaserScanStampedPtr & _msg) { - // Create message to send - sensor_msgs::msg::PointCloud pc; - - // Fill header + // Convert Laser scan to PointCloud + auto pc = gazebo_ros::Convert(*_msg, min_intensity_); + // Set tf frame pc.header.frame_id = frame_name_; - pc.header.stamp.sec = _msg->time().sec(); - pc.header.stamp.nanosec = _msg->time().nsec(); - - // Setup point cloud fields - pc.points.reserve(verticalRangeCount * rangeCount); - pc.channels.push_back(sensor_msgs::msg::ChannelFloat32()); - pc.channels[0].values.reserve(verticalRangeCount * rangeCount); - pc.channels[0].name = "intensity"; - - // Fill pointcloud with laser scan - for (int i = 0; i < rangeCount; i++) { - // Calculate azimuth (horizontal angle) - double azimuth; - if (rangeCount > 1) { - azimuth = i * yDiff / (rangeCount - 1) + minAngle; - } else { - azimuth = minAngle; - } - double c_azimuth = cos(azimuth); - double s_azimuth = sin(azimuth); - - for (int j = 0; j < verticalRangeCount; ++j) { - double inclination; - // Calculate inclination (vertical angle) - if (verticalRayCount > 1) { - inclination = j * pDiff / (verticalRangeCount - 1) + verticalMinAngle; - } else { - inclination = verticalMinAngle; - } - double c_inclination = cos(inclination); - double s_inclination = sin(inclination); - - // Get range and intensity at this scan - size_t index = i + j * rangeCount; - double r = _msg->scan().ranges(index); - double intensity = _msg->scan().intensities(index); - if (intensity < this->min_intensity_) { - intensity = this->min_intensity_; - } - - // Convert spherical coordinates to cartesian for pointcloud - // See https://en.wikipedia.org/wiki/Spherical_coordinate_system - geometry_msgs::msg::Point32 point; - point.x = r * c_inclination * c_azimuth; - point.y = r * c_inclination * s_azimuth; - point.z = r * s_inclination; - pc.points.push_back(point); - pc.channels[0].values.push_back(intensity); - } - } - // Publish output boost::get(pub_)->publish(pc); } void GazeboRosRaySensorPrivate::PublishPointCloud2(ConstLaserScanStampedPtr & _msg) { - // Create message to send - sensor_msgs::msg::PointCloud2 pc; - - // Fill header - pc.header.frame_id = frame_name_; - pc.header.stamp.sec = _msg->time().sec(); - pc.header.stamp.nanosec = _msg->time().nsec(); - - // Create fields in pointcloud - sensor_msgs::PointCloud2Modifier pcd_modifier(pc); - pcd_modifier.setPointCloud2Fields(4, - "x", 1, sensor_msgs::msg::PointField::FLOAT32, - "y", 1, sensor_msgs::msg::PointField::FLOAT32, - "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); - pcd_modifier.resize(verticalRangeCount * rangeCount); - pc.is_dense = true; - sensor_msgs::PointCloud2Iterator iter_x(pc, "x"); - sensor_msgs::PointCloud2Iterator iter_y(pc, "y"); - sensor_msgs::PointCloud2Iterator iter_z(pc, "z"); - sensor_msgs::PointCloud2Iterator iter_intensity(pc, "intensity"); - - // Fill pointcloud with laser scan - for (int i = 0; i < rangeCount; i++) { - // Calculate azimuth (horizontal angle) - double azimuth; - if (rangeCount > 1) { - azimuth = i * yDiff / (rangeCount - 1) + minAngle; - } else { - azimuth = minAngle; - } - double c_azimuth = cos(azimuth); - double s_azimuth = sin(azimuth); - - for (int j = 0; j < verticalRangeCount; - ++j, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity) - { - double inclination; - // Calculate inclination (vertical angle) - if (verticalRayCount > 1) { - inclination = j * pDiff / (verticalRangeCount - 1) + verticalMinAngle; - } else { - inclination = verticalMinAngle; - } - double c_inclination = cos(inclination); - double s_inclination = sin(inclination); - - // Get range and intensity at this scan - size_t index = i + j * rangeCount; - double r = _msg->scan().ranges(index); - double intensity = _msg->scan().intensities(index); - if (intensity < min_intensity_) { - intensity = min_intensity_; - } - - // Convert spherical coordinates to cartesian for pointcloud - // See https://en.wikipedia.org/wiki/Spherical_coordinate_system - *iter_x = r * c_inclination * c_azimuth; - *iter_y = r * c_inclination * s_azimuth; - *iter_z = r * s_inclination; - *iter_intensity = intensity; - } - } - + // Convert Laser scan to PointCloud2 + auto pc2 = gazebo_ros::Convert(*_msg, min_intensity_); + // Set tf frame + pc2.header.frame_id = frame_name_; // Publish output - boost::get(pub_)->publish(pc); + boost::get(pub_)->publish(pc2); } void GazeboRosRaySensorPrivate::PublishRange(ConstLaserScanStampedPtr & _msg) { - sensor_msgs::msg::Range range_msg; + // Convert Laser scan to PointCloud2 + auto range_msg = gazebo_ros::Convert(*_msg); + // Set tf frame range_msg.header.frame_id = frame_name_; - range_msg.header.stamp.sec = _msg->time().sec(); - range_msg.header.stamp.nanosec = _msg->time().nsec(); - range_msg.field_of_view = std::max(pDiff, yDiff); - range_msg.min_range = rangeMin; - range_msg.max_range = rangeMax; + // Set radation type from sdf range_msg.radiation_type = range_radiation_type_; - - // Set range to the minimum of the ray ranges - // For single rays, this will just be the range of the ray - range_msg.range = std::numeric_limits::max(); - for (double range : _msg->scan().ranges()) { - if (range < range_msg.range) { - range_msg.range = range; - } - } - + // Publish output boost::get(pub_)->publish(range_msg); } diff --git a/gazebo_plugins/test/worlds/gazebo_ros_ray_sensor.world b/gazebo_plugins/test/worlds/gazebo_ros_ray_sensor.world index 260a95f04..0778d98b7 100644 --- a/gazebo_plugins/test/worlds/gazebo_ros_ray_sensor.world +++ b/gazebo_plugins/test/worlds/gazebo_ros_ray_sensor.world @@ -26,13 +26,11 @@ 300 - 1.0 -0.5236 0.5236 100 - 1.0 -0.5236 0.5236 @@ -51,7 +49,7 @@ /ray ~/out:=pointcloud2 - sensor_msgs/PointCloud2 + sensor_msgs/PointCloud2 @@ -59,7 +57,7 @@ /ray ~/out:=pointcloud - sensor_msgs/PointCloud + sensor_msgs/PointCloud @@ -67,7 +65,7 @@ /ray ~/out:=laserscan - sensor_msgs/LaserScan + sensor_msgs/LaserScan @@ -75,7 +73,7 @@ /ray ~/out:=range - sensor_msgs/Range + sensor_msgs/Range