Add Compound Sensors

[DavidPL1]

This PR, based on the discussion in #26, overhauls the MuJoCo sensor to ROS serialization and introduces compound sensors.

The currently available compound sensors are Twist, IMU, Wrench, and Pose. New ones can easily be added.

Points to Discuss

• Is a factory for compound sensors overkill or worth the reduced maintenance cost?
• Sensors used in compound sensors are not separately published
• Should the frame ids of sensors used in a compound sensor always be the same?
• Defining compound sensors requires a list of
  • sensors
  • (frame_id? For now we assume the frame id of the included sensors which should be coherent)
  • frequency
  • type
  • name
  • publishing rate (at some point when the feature is introduced) Since the custom tag does not really allow adding all this information conveniently, I moved the whole configuration to the plugin configuration.

TODOs

• [ ] IMU covariance configuration is NYI
• [ ] Tests for compound sensors

[DavidPL1]

@balandbal, this is what I came up with based on our discussion. Could you take a look and tell me what you think? I'd also appreciate your take on the points of discussion mentioned above.

[codecov[bot]]

Codecov Report

Attention: 238 lines in your changes are missing coverage. Please review.

Comparison is base (455e135) 71.43% compared to head (86f691c) 61.87%. Report is 2 commits behind head on noetic-devel.

Files	Patch %	Lines
mujoco_ros_sensors/src/compound_sensors.cpp	0.00%	169 Missing :warning:
...o_ros_sensors/src/mujoco_sensor_handler_plugin.cpp	48.70%	59 Missing :warning:
mujoco_ros_sensors/src/serialization.cpp	91.57%	7 Missing :warning:
...o_ros_sensors/include/mujoco_ros_sensors/sensors.h	91.43%	3 Missing :warning:

Additional details and impacted files

```diff @@ Coverage Diff @@ ## noetic-devel #32 +/- ## ================================================ - Coverage 71.43% 61.87% -9.56% ================================================ Files 10 12 +2 Lines 1414 1602 +188 ================================================ - Hits 1010 991 -19 - Misses 404 611 +207 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[balandbal]

• Sensors used in compound sensors are not separately published

I'd say this is reasonable behaviour. I don't remember a case when I needed the components of, e.g., a wrench signal separately available in ROS at the same time I used the compound signal.

• Should the frame ids of sensors used in a compound sensor always be the same?

I'd say yes. These compound signals should correspond to those from some real-world hardware; I'm not aware of a tool that would be "distributed" in the sense that it would require different frames to model.

• Defining compound sensors requires a list of
  • sensors
  • (frame_id? For now we assume the frame id of the included sensors which should be coherent)
  • frequency
  • type
  • name
  • publishing rate (at some point when the feature is introduced) Since the custom tag does not really allow adding all this information conveniently, I moved the whole configuration to the plugin configuration.

I'd be happier if the grouping of the sensors could move with the MJCF and not separately, though this is because I usually use my models with, e.g., the official Python bindings as well and not only mujoco_ros.

The mujoco_ros_sensors/config/example_sensors.yaml file would look something like the XML below with the custom tag. We could reserve the names for the compound sensor types and use the prm attribute to store the frequencies.

<custom>
  <tuple name="IMUSensor">
    <element objname="test_imu"   objtype="tuple" prm="500" />
    <element objname="test_imu_2" objtype="tuple" />
  </tuple>

  <tuple name="WrenchSensor">
    <element objname="test_wrench" objtype="tuple" prm="500"/>
  </tuple>

  <tuple name="TwistSensor">
    <element objname="test_twist" objtype="tuple" prm="500"/>
  </tuple>

  <tuple name="PoseSensor">
    <element objname="test_pose" objtype="tuple" prm="250" />
  </tuple>

  <tuple name="test_imu">
    <element objname="linacc_EE" objtype="sensor"/>
    <element objname="gyro_EE"   objtype="sensor"/>
    <element objname="quat_EE"   objtype="sensor"/>
  </tuple>

  <tuple name="test_imu_2">
    <element objname="linacc_2" objtype="sensor"/>
    <element objname="gyro_2"   objtype="sensor"/>
    <element objname="quat_2"   objtype="sensor"/>
  </tuple>

  <tuple name="test_wrench">
    <element objname="force_EE"  objtype="sensor"/>
    <element objname="torque_EE" objtype="sensor"/>
  </tuple>

  <tuple name="test_twist">
    <element objname="linvel_EE" objtype="sensor"/>
    <element objname="angvel_EE" objtype="sensor"/>
  </tuple>

  <tuple name="test_pose">
    <element objname="pos_joint1"  objtype="sensor"/>
    <element objname="quat_joint1" objtype="sensor"/>
  </tuple>

</custom>

[DavidPL1]

• Sensors used in compound sensors are not separately published

I'd say this is reasonable behaviour. I don't remember a case when I needed the components of, e.g., a wrench signal separately available in ROS at the same time I used the compound signal.

Alright then let's keep this as is.

• Should the frame ids of sensors used in a compound sensor always be the same?

I'd say yes. These compound signals should correspond to those from some real-world hardware; I'm not aware of a tool that would be "distributed" in the sense that it would require different frames to model.

What I was worrying about was correct reference frames of the sensor readings. Torques, for instance, only make sense if their reference frame, i.e., their frame_id in ROS, is set correctly. Since a compound sensor could technically consist of arbitrarily far apart sensors, a misconfigured torque sensor could send torques in reference to its local frame instead of the compound sensor frame. I guess we could pin the responsibility of correct configuration to the user, but I'd be happier with sanity checks. With this in mind, I think it makes sense to use a common frame_id and add a check that each sensor is attached to the same site (or reftype and refobject in case of frame-related sensors). Agreed?

I'd be happier if the grouping of the sensors could move with the MJCF and not separately, though this is because I usually use my models with, e.g., the official Python bindings as well and not only mujoco_ros.

I was unhappy with the separated definition, too. I was not aware that one could add more than two children to a tuple element and that was the main reason I did not consider this way of configuration. My mistake, I'll try and adapt the code to use this.

[balandbal]

What I was worrying about was correct reference frames of the sensor readings. Torques, for instance, only make sense if their reference frame, i.e., their frame_id in ROS, is set correctly. Since a compound sensor could technically consist of arbitrarily far apart sensors, a misconfigured torque sensor could send torques in reference to its local frame instead of the compound sensor frame. I guess we could pin the responsibility of correct configuration to the user, but I'd be happier with sanity checks. With this in mind, I think it makes sense to use a common frame_id and add a check that each sensor is attached to the same site (or reftype and refobject in case of frame-related sensors). Agreed?

Yes, I would be happy with that.