[ros2] Port gazebo_ros_imu_sensor #793

kev-the-dev · 2018-08-07T23:04:23Z

Port the gazebo_ros_imu_sensor to ROS2, following the new conventions.

A wiki page has been created to guide users in migrating.

dhood

Just a partial review so far

dhood · 2018-08-10T00:46:31Z

gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.hpp

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


could add override

dhood · 2018-08-10T00:50:13Z

gazebo_plugins/src/gazebo_ros_imu_sensor.cpp

+{
+public:
+  /// Node for ros communication
+  gazebo_ros::Node::SharedPtr rosnode_;


ros_node_ to match e.g.

gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp

Line 58 in 4fe1967

gazebo_ros::Node::SharedPtr ros_node_;

dhood · 2018-08-10T02:09:36Z

gazebo_plugins/src/gazebo_ros_imu_sensor.cpp

+    return;
+  }
+
+  impl_->pub_ = impl_->rosnode_->create_publisher<sensor_msgs::msg::Imu>("~/out");


I will say it explicitly in case anyone disagrees with me: though we have a sensor_data QoS profile I'd say that's more appropriate for subscribers. It's better for this to publish as reliable so that reliable subscribers will still be able to connect.

dhood · 2018-08-10T18:47:46Z

gazebo_plugins/src/gazebo_ros_imu_sensor.cpp

+  impl_->pub_ = impl_->rosnode_->create_publisher<sensor_msgs::msg::Imu>("~/out");
+
+  // Create message to be reused
+  auto msg = std::make_shared<sensor_msgs::msg::Imu>();


@mikaelarguedas can you confirm that this will initialise the other elements of the covariance matrix fields to 0?
From ros2/geometry2#64 I think the answer is yes, but http://design.ros2.org/articles/generated_interfaces_cpp.html says the default constructor causes default/value initialised, and doesn't say the default is zero initialised.

TL;DR: yes all the fields of the array will be set to zero

Longer answer:
From the doc: The default constructor initializes all members with their default value; if a field doesn’t have a default value, then the member is value-initialized.

As Imu doesn't provide custom default values for its' fields (https://github.com/ros2/common_interfaces/blob/60afcc4b3256d5a0f606939a5d02e173ff15bdd9/sensor_msgs/msg/Imu.msg#L18) they will initialized with ROS' default.

These default are language independent so they are defined in the Interface Definition doc.

A field can optionally specify a default value. If no default value is specified a common default value is used: for bool it is false for numeric types it is a 0 value for string it is an empty string for static size arrays it is an array of N elements with its fields zero-initialized for bounded size arrays and dynamic size arrays it is an empty array []

The part we're interested in here is for static size arrays it is an array of N elements with its fields zero-initialized

mmmm I didn't realise from the design article that it would use a ROS default and not just a default specified in the .msg. I will try to update that design doc based on your answer, thanks!!

dhood · 2018-08-10T21:23:12Z

gazebo_plugins/test/worlds/gazebo_ros_imu_sensor.world

+        <sensor name="my_imu" type="imu">
+          <always_on>true</always_on>
+          <update_rate>30</update_rate>
+          <imu>


if this empty tag is needed could you add a comment why?

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

@ros-pull-request-builder retest this please

dhood · 2018-08-17T20:59:08Z

After speaking with ironmig offline I reworked the test to not need the step_for_msgs function anymore, following the ros laser test instead: it now steps forward once: https://github.com/ros-simulation/gazebo_ros_pkgs/pull/793/files#diff-d966e67ca1b4db645730d08da8e16054R57

I also made it apply the force for multiple steps, and now the test checks the magnitude of the values.

I noticed that the linear acceleration flips between positive and negative, though this happens with the readings of the IMU directly from gazebo, it's not a conversion problem. Not sure if that happens with the gazebo_ros_imu plugin too, but it's not something that's been introduced in the porting of this plugin. Example of the values during the test:

9: Angular velocity: 1.443684
9: Linear acceleration: 7.542811
9: Yaw: 0.167626
9: Angular velocity: 1.808677
9: Linear acceleration: 7.469551
9: Yaw: 0.220460
9: Angular velocity: 1.583626
9: Linear acceleration: -3.116650
9: Yaw: 0.276416
9: Angular velocity: 1.948953
9: Linear acceleration: 7.004602
9: Yaw: 0.344101
9: Angular velocity: 2.277386
9: Linear acceleration: 7.697588
9: Yaw: 0.414009
9: Angular velocity: 2.024620
9: Linear acceleration: -4.554927
9: Yaw: 0.489143
9: Angular velocity: 2.360117
9: Linear acceleration: 7.322274
9: Yaw: 0.572661
9: Angular velocity: 2.605397
9: Linear acceleration: 6.003855
9: Yaw: 0.657134
9: Angular velocity: 2.964912
9: Linear acceleration: 6.983869
9: Yaw: 0.741163
9: Angular velocity: 2.702542
9: Linear acceleration: -6.040410
9: Yaw: 0.840170

dhood · 2018-08-17T21:23:54Z

gazebo_plugins/src/gazebo_ros_imu_sensor.cpp

+  // TODO(anyone): covariance for IMU's orientation once this is added to gazebo
+  using SNT = gazebo::sensors::SensorNoiseType;
+  msg->angular_velocity_covariance[0] =
+    gazebo_ros::NoiseVariance(impl_->sensor_->Noise(SNT::IMU_ANGVEL_X_NOISE_RADIANS_PER_S));


I'm seeing [Err] [Sensor.cc:454] Get noise index not valid from these lines. looking into why.

it's caused by https://bitbucket.org/osrf/gazebo/issues/2513/ in general, but even if the noise is specified in the SDF, the error still shows in tests because the noise isn't applied via the ServerFixture: #799

dhood · 2018-08-17T23:20:56Z

Followup work will be to prevent the noise error mentioned in https://github.com/ros-simulation/gazebo_ros_pkgs/pull/793/files#r211036103: it's caused by https://bitbucket.org/osrf/gazebo/issues/2513/ in general, but even if the noise is specified in the SDF, the error still shows in tests because the noise isn't applied via the ServerFixture: #799. We don't have to let that block this PR because the plugin is working as expected (noise applied when specified), it just prints an unnecessary error if launched with verbose e.g.

gazebo ~/ros2_ws/src/ros2/gazebo_ros_pkgs/gazebo_plugins/test/worlds/gazebo_ros_imu_sensor.world --verbose

dhood · 2018-08-20T21:29:33Z

@chapulina realised that the test was behaving as expected from a clean build. #799 turned out to be a non-issue, just user error.

#801 added noise to the sensor used in the test so the error doesn't show in the test output, but sensors using this plugin will still have error output if the noise isn't specified: https://bitbucket.org/osrf/gazebo/issues/2513/

kev-the-dev force-pushed the ros2-utils branch from 63f0426 to 5ce7d41 Compare August 7, 2018 23:30

kev-the-dev force-pushed the ros2-imu-sensor branch from 397882f to 02d4213 Compare August 7, 2018 23:31

j-rivero mentioned this pull request Aug 7, 2018

Port gazebo_ros_pkgs to ROS 2.0 #512

Closed

kev-the-dev changed the base branch from ros2-utils to ros2 August 8, 2018 00:40

kev-the-dev force-pushed the ros2-imu-sensor branch 3 times, most recently from 96d3147 to d58b7df Compare August 8, 2018 16:52

dhood reviewed Aug 10, 2018

View reviewed changes

Kevin Allen added 3 commits August 10, 2018 13:36

Move files to prepare for imu_sensor ROS2 port

6106b41

Port gazebo_ros_imu_sensor

ecf2850

Address IMU Sensor PR comments

fafc2e8

kev-the-dev force-pushed the ros2-imu-sensor branch from 896d976 to fafc2e8 Compare August 10, 2018 20:36

dhood reviewed Aug 10, 2018

View reviewed changes

dhood mentioned this pull request Aug 10, 2018

Clarify ROS default values will be used in default constructor ros2/design#184

Merged

Remove empty <imu> tag

1d618ae

kev-the-dev force-pushed the ros2-imu-sensor branch from 392aea0 to 1d618ae Compare August 10, 2018 23:48

dhood added 5 commits August 17, 2018 13:36

document that always_on is required

f21a7ea

alphabetical order includes

ce92d5c

Step far forward instead of multiple small steps

effb9f2

Fix test_conversions not finding quaternion.hpp

80f3a78

Apply force longer; check IMU values; robust to negative linear accel

24cc5ca

linter fixup

65dfe66

dhood reviewed Aug 17, 2018

View reviewed changes

dhood approved these changes Aug 17, 2018

View reviewed changes

dhood added the ros2 label Aug 17, 2018

dhood merged commit e9535ad into ros2 Aug 17, 2018

dhood deleted the ros2-imu-sensor branch August 17, 2018 23:59

This pull request was closed.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[ros2] Port gazebo_ros_imu_sensor #793

[ros2] Port gazebo_ros_imu_sensor #793

kev-the-dev commented Aug 7, 2018 •

edited

Loading

dhood left a comment

dhood Aug 10, 2018

dhood Aug 10, 2018

dhood Aug 10, 2018

dhood Aug 10, 2018

mikaelarguedas Aug 10, 2018

dhood Aug 10, 2018

dhood Aug 10, 2018

mikaelarguedas commented Aug 13, 2018

dhood commented Aug 17, 2018

dhood Aug 17, 2018

dhood Aug 17, 2018

dhood commented Aug 17, 2018

dhood commented Aug 20, 2018

[ros2] Port gazebo_ros_imu_sensor #793

[ros2] Port gazebo_ros_imu_sensor #793

Conversation

kev-the-dev commented Aug 7, 2018 • edited Loading

dhood 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

mikaelarguedas commented Aug 13, 2018

dhood commented Aug 17, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dhood commented Aug 17, 2018

dhood commented Aug 20, 2018

kev-the-dev commented Aug 7, 2018 •

edited

Loading