[ros2] Add gazebo_ros_ray_sensor #785

kev-the-dev · 2018-07-31T01:20:01Z

Add gazebo_ros_ray_sensor, a plugin to replace gazebo_ros_laser, gazebo_ros_gpu_laser, gazebo_ros_block_laser, and gazebo_ros_range. It attaches to a ray or gpu_ray sensor and publishes an output message in ROS based with the type configured via an sdf parameter.

~~The test is currently non-exhaustive and only checks the content of the range type, ensuring that that all types just publish messages. I will be adding more extensive tests.~~

chapulina

I've done a first high level pass over the code.

There are a few changes to the math that I'd like to take a closer look at. If we want to say with some degree of confidence that users should be able to migrate from some specific old sensor type to this new one, we will need to:

Write detailed migration guides explaining, for example:
- what parameters changed
- how noise can be configured to behave the same way as before
- any expected changes in sensor output, and why
Add some tests that show behaviour is still the same

I realize these tasks can become arbitrarily long. We need to weigh the effort of providing a robust migration path against the effort of maintaining, at least for now, 4 different plugins that do similar things but at least behave as they did in ROS 1.

Let's start a dedicated wiki page for the migration documentation: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-Ray-sensors

chapulina · 2018-07-31T21:31:57Z

gazebo_plugins/CMakeLists.txt

    gazebo_ros_force
    gazebo_ros_template
+    gazebo_ros_ray_sensor


It would be convenient to keep these in alphabetical order

chapulina · 2018-07-31T21:32:42Z

gazebo_plugins/include/gazebo_plugins/gazebo_ros_ray_sensor.hpp

+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/range.hpp>
+#include <gazebo_ros/node.hpp>
+#include <boost/variant.hpp>


Most of these includes can be (re)moved

Done in 63a3e1f

chapulina · 2018-07-31T21:38:52Z

gazebo_plugins/include/gazebo_plugins/gazebo_ros_ray_sensor.hpp

+  \details
+  SDF parameters:
+  \verbatim
+  <outputType>: Optional. The message type of the plugin's output. Can be any of the following:


It always seemed odd to me that some of the tags were camelCase. Both URDF and SDF use the underscore_case convention, and gazebo_ros::Node, as of now, is also using that.

So I think it would be nice to also convert all tags to this style. Thoughts?

This has been done

chapulina · 2018-07-31T21:39:53Z

gazebo_plugins/include/gazebo_plugins/gazebo_ros_ray_sensor.hpp

+
+  Example SDF:
+  \code{.xml}
+    <plugin name="my_ray_senor_plugin" filename="libgazebo_ros_ray_sensor.so">


chapulina · 2018-07-31T21:41:01Z

gazebo_plugins/include/gazebo_plugins/gazebo_ros_ray_sensor.hpp

+  virtual ~GazeboRosRaySensor();
+
+  // Documentation Inherited
+  void Load(gazebo::sensors::SensorPtr _parent, sdf::ElementPtr _sdf);


Can remove _ prefix

I'm now leaning towards leaving this in, sorry for the roundabout, as far as I know, this doesn't conflict with any ROS2 styles, and it's convenient.

chapulina · 2018-07-31T23:07:51Z

gazebo_plugins/src/gazebo_ros_ray_sensor.cpp

+  /// Gazebo subscribe to parent sensor's laser scan
+  gazebo::transport::SubscriberPtr laser_scan_sub_;
+
+  int rayCount;


Double-check which of these variables are actually needed and:

remove the unused ones

document and add _ suffix to the used ones

It's been cleanup

chapulina · 2018-07-31T23:39:00Z

gazebo_plugins/src/gazebo_ros_ray_sensor.cpp

+  }
+}
+
+void GazeboRosRaySensorPrivate::PublishLaserScan(ConstLaserScanStampedPtr & _msg)


Do you think it makes sense to have functions like sensor_msgs::msg::LaserScan Convert(const gazebo::msgs::LaserScanStamped & msg) which do most of the heavy-lifting and then the plugin just sets extra stuff like frame_id?

That would be a lot cleaner. I have to first make sure the gazebo message has everything needed to do the conversion, but this sounds like a good refactor

It's been addressed

chapulina · 2018-07-31T23:42:06Z

gazebo_plugins/src/gazebo_ros_ray_sensor.cpp

+{
+}
+
+void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _parent, sdf::ElementPtr _sdf)


Consider renaming _parent to sensor, so it is clear that it isn't the parent model or link for example.

chapulina · 2018-08-01T00:23:08Z

gazebo_plugins/src/gazebo_ros_ray_sensor.cpp

+  size_t start = (verticalRangeCount / 2) * rangeCount;
+
+  // Copy ranges and intensities over
+  ls.ranges.resize(rangeCount);


Why not use _msg->scan().ranges_size()?

I think this would be a problem if there is greater than one vertical ray as this is just a planar scan. rangeCount is the number of horizontal rays, where as _msg->scan().ranges_size() is all the rays

chapulina · 2018-08-01T00:31:17Z

gazebo_plugins/src/gazebo_ros_ray_sensor.cpp

+  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);


On gazebo_ros_range the FOV is set from the SDF but now it comes from the sensor itself. If the <fov> param wasn't useful, let's document it.

It's been documented

kev-the-dev · 2018-08-01T18:13:33Z

Migration notes with examples added to the wiki. I'm going to recommit the conversion/utility functions in a separate PR than come back to address the specific issues for gazebo_ros_ray_sensor

chapulina · 2018-08-09T00:48:32Z

gazebo_plugins/src/gazebo_ros_ray_sensor.cpp

+
+void GazeboRosRaySensorPrivate::PublishRange(ConstLaserScanStampedPtr & _msg)
+{
+  // Convert Laser scan to PointCloud2


PointCloud2 -> Range

chapulina · 2018-08-09T00:58:34Z

gazebo_plugins/test/test_gazebo_ros_ray_sensor.cpp

+  /// ROS node used to verify output of plugin
+  static rclcpp::Node::SharedPtr node_;
+  /// ROS executor to call node_'s callbacks
+  static rclcpp::executor::Executor::SharedPtr executor_;


Just curious, why are you making these static?

Rewrote test to actually verify correctness of output using gazebo's ground truth in
3f117fe

chapulina · 2018-08-09T01:44:41Z

@ironmig , could you also add a demo world besides the test world? On the world, add comments explaining how to launch RViz to visualize the various sensors.

kev-the-dev · 2018-08-10T20:28:23Z

Added the test world and expanded the test to verify the correctness of the message content's using gazebo's ground truth

mikaelarguedas · 2018-08-13T20:16:08Z

@ros-pull-request-builder retest this please

chapulina · 2018-08-21T03:15:55Z

Thanks for the demo world, @ironmig! I added a few more examples to it on 9c74f9c and I noticed some possible problems which I need to investigate further:

I couldn't get the LaserScan messages to show on RViz for either ray or gpu_ray sensors. The Status is ok, messages are received, but it always says "Showing [0] points from [0] messages". We could be forgetting to publish some field? Or maybe it's RViz's fault? I see stuff when echoing the messages.
When I have only one vertical ray, the point cloud's position looks wrong. See how the 1-vertical-ray gpu_ray is way below the output for the 300-vertical-rays ray sensor:

This happens even if I put one vertical ray for the ray sensor, note how the intensities are properly colored but the result is also under the ground:

approved by mistake

chapulina · 2018-08-21T17:26:00Z

The issue with the LaserScan not being visible on RViz seems to be due to ros2/rviz#332

chapulina · 2018-08-21T20:26:03Z

The issue with 1 vertical ray was happening if there was a <vertical> tag with 1 sample, but not if there was no <vertical> tag. So I changed the example world and also added a warning.

I think this is good to go if the CI comes clean. I'll add a note to the migration page about #778 (comment) by @dhood.

chapulina suggested changes Aug 1, 2018

View reviewed changes

kev-the-dev force-pushed the ros2-ray-sensor branch from 01c70f9 to 0cd7cad Compare August 1, 2018 21:43

kev-the-dev changed the base branch from ros2 to ros2-utils August 1, 2018 21:43

kev-the-dev force-pushed the ros2-ray-sensor branch from 0cd7cad to ff2bc88 Compare August 6, 2018 17:58

kev-the-dev changed the base branch from ros2-utils to ros2 August 6, 2018 23:49

kev-the-dev force-pushed the ros2-ray-sensor branch from ff2bc88 to b77a942 Compare August 6, 2018 23:52

j-rivero mentioned this pull request Aug 7, 2018

Port gazebo_ros_pkgs to ROS 2.0 #512

Closed

chapulina reviewed Aug 9, 2018

View reviewed changes

kev-the-dev force-pushed the ros2-ray-sensor branch 3 times, most recently from 9ffcf8f to 4916e3f Compare August 10, 2018 20:23

dhood added the ros2 label Aug 17, 2018

Kevin Allen and others added 10 commits August 20, 2018 18:49

Delete files for old range plugins

2ba6ec2

Add gazebo_ros_ray_sensor

9f71475

Add LaserScan conversions to gazebo_ros

e48c807

Simplify ray_sensor using gazebo_ros conversions

0b88b30

Add Point32->ign vector conversion, fix pointcloud conversion

c01a26b

Verify correctness of gazebo_ros_ray_sensor output

367bd52

Add ray_sensor demo

d37c1fe

Test correctness of ray_sensor intensity

783d863

fix build by adding includes

2482816

tweaks to includes

c39e1c1

chapulina force-pushed the ros2-ray-sensor branch from 63a3e1f to c39e1c1 Compare August 21, 2018 01:54

Add more examples, need to debug some

9c74f9c

chapulina previously approved these changes Aug 21, 2018

View reviewed changes

working demo, notes and warnings about issues

d0a86d8

chapulina merged commit 9c9f219 into ros2 Aug 21, 2018

dhood deleted the ros2-ray-sensor branch August 21, 2018 21:40

dhood mentioned this pull request Aug 22, 2018

[ros2] Split conversions into headers specific to message packages #803

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[ros2] Add gazebo_ros_ray_sensor #785

[ros2] Add gazebo_ros_ray_sensor #785

kev-the-dev commented Jul 31, 2018 •

edited

Loading

chapulina left a comment

chapulina Jul 31, 2018

chapulina Jul 31, 2018

chapulina Aug 18, 2018

chapulina Jul 31, 2018

kev-the-dev Aug 1, 2018

chapulina Aug 15, 2018

chapulina Jul 31, 2018

chapulina Aug 18, 2018

chapulina Jul 31, 2018

chapulina Aug 15, 2018

chapulina Jul 31, 2018

chapulina Aug 15, 2018

chapulina Jul 31, 2018

kev-the-dev Aug 1, 2018

chapulina Aug 18, 2018

chapulina Jul 31, 2018

chapulina Aug 15, 2018

chapulina Aug 1, 2018

kev-the-dev Aug 1, 2018

chapulina Aug 1, 2018

chapulina Aug 15, 2018

kev-the-dev commented Aug 1, 2018

chapulina Aug 9, 2018

chapulina Aug 15, 2018

chapulina Aug 9, 2018

kev-the-dev Aug 9, 2018

chapulina commented Aug 9, 2018

kev-the-dev commented Aug 10, 2018

mikaelarguedas commented Aug 13, 2018

chapulina commented Aug 21, 2018

chapulina commented Aug 21, 2018

chapulina commented Aug 21, 2018

[ros2] Add gazebo_ros_ray_sensor #785

[ros2] Add gazebo_ros_ray_sensor #785

Conversation

kev-the-dev commented Jul 31, 2018 • edited Loading

chapulina left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kev-the-dev commented Aug 1, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

chapulina commented Aug 9, 2018

kev-the-dev commented Aug 10, 2018

mikaelarguedas commented Aug 13, 2018

chapulina commented Aug 21, 2018

chapulina commented Aug 21, 2018

chapulina commented Aug 21, 2018

kev-the-dev commented Jul 31, 2018 •

edited

Loading