Cannot use diff_drive_controller with latest version of gazebo_ros2_control

[joshnewans]

Symptoms

When attempting to use a diff_drive_controller with the latest version of gazebo_ros2_control (i.e. merging @bmagyar's PRs https://github.com/ros-simulation/gazebo_ros2_control/pull/89 and https://github.com/ros-simulation/gazebo_ros2_control/pull/88), the update function fails with the following error:

[gzserver-2]   what():  can't subtract times with different time sources [2 != 1]

Immediate Cause

The issue seems to be that since update() now has time passed into it (which is soon turned into current_time), there is an inconsistency between rcl_clock_types.

https://github.com/ros-controls/ros2_controllers/blob/8e40fd5791805e3d1435992b5134673aecddc25b/diff_drive_controller/src/diff_drive_controller.cpp#L135-L136

The following two snippets are both points where a Time with the clock_type set to RCL_ROS_TIME (i.e. 1) is subtracted from RCL_SYSTEM_TIME (i.e. 2).

https://github.com/ros-controls/ros2_controllers/blob/8e40fd5791805e3d1435992b5134673aecddc25b/diff_drive_controller/src/diff_drive_controller.cpp#L160

https://github.com/ros-controls/ros2_controllers/blob/8e40fd5791805e3d1435992b5134673aecddc25b/diff_drive_controller/src/diff_drive_controller.cpp#L240

Root Cause

I believe the root cause of this is that different rclcpp::Time constructors use a different default clock (presumably for good reason) https://docs.ros2.org/galactic/api/rclcpp/classrclcpp_1_1Time.html.

The Time object created by gazebo_ros2_control and passed down to the controller will have the RCL_SYSTEM_TIME type, whereas ones constructed from messages will have RCL_ROS_TIME type. https://github.com/ros-simulation/gazebo_ros2_control/blob/04b2c6f0ff0e977b6fc6af5dfc5e96e5bdd570d0/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp#L368

Solution

Without a strong understanding of the libraries, I would assume that the correct solution is to modify gazebo_ros2_control to use RCL_ROS_TIME. I made a quick attempt at this which broke a bunch of other things, so it will need to be done carefully.

The alternative is to modify diff_drive_controller to user RCL_SYSTEM_TIME. I had a go at this too (changing the two lines referenced above) and it successfully compiled and ran, however my controller does not seem to work (may be unrelated issue).

(Sorry if this is formatted incorrectly, after I added the snippets it wasn't rendering the issue template correctly so I rearranged it into headings)

[bmagyar]

Thanks for the analysis on this. We've discussed this regarding these changes and it seems that the consensus is to use ROS time almost always. This allows to support both simulation time or non-simulation time which are ground we both aim to cover with the framework & controllers.

I had to touch some of the unit tests up too to take this into account: https://github.com/ros-controls/ros2_controllers/pull/241/files#diff-4497637c95ad8ef8fcbd6d9ab5ab4c77219aa1460c31521288095c5d659f7129R289

[joshnewans]

That's pretty much what I expected. So should I create an issue in gazebo_ros2_control, seeing as it will need to be changed? I'm not sure since the issue will only be manifest after your PR is merged in, so perhaps your PR should be amended first?

Edit: Specifically, that sim_time_ros in gazebo_ros2_control_plugin.cpp needs to use RCL_ROS_TIME, which for me spawned other errors which I wasn't game to try and fix with my current level of experience.

[rcywongaa]

This is also happening for ign_ros2_control. Though in this case, the symptom is an assertion fail

[ign gazebo-2] ign gazebo server: /opt/ros/galactic/include/rcppmath/clamp.hpp:41: constexpr const T& rcppmath::clamp(const T&, const T&, const T&) [with T = double]: Assertion `!(hi < lo)' failed.

https://github.com/ros-controls/ros2_controllers/blob/8e40fd5791805e3d1435992b5134673aecddc25b/diff_drive_controller/src/diff_drive_controller.cpp#L479

Sets previous_update_timestamp_ to system (epoch) time

https://github.com/ros-controls/ros2_controllers/blob/8e40fd5791805e3d1435992b5134673aecddc25b/diff_drive_controller/src/diff_drive_controller.cpp#L240-L248

Attempts to subtract sim time with system time leading to a negative update_dt which causes the limit() functions to fail assertions.

https://github.com/ros-controls/ros2_controllers/pull/267 solves the issue