Make diff_drive_controller a ChainableControllerInterface #1485
Conversation
github-actions
bot
requested review from
aprotyas,
ARK3r,
bmagyar,
destogl,
mamueluth,
Raivias and
rosterloh
|
Please always install pre-commit to avoid failing tests ;) otherwise, run it manually with regarding tests: have a look at the mecanum controller, it is chainable anf there are exactly those tests already available |
arthurlovekin
force-pushed
the
diff-drive-chainable-1457
branch
from
19b7630 to
a353628
Compare
|
I've fixed my formatting by running |
Codecov ReportAttention: Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## master #1485 +/- ##
==========================================
+ Coverage 83.86% 84.09% +0.22%
==========================================
Files 122 121 -1
Lines 11139 11247 +108
Branches 945 949 +4
==========================================
+ Hits 9342 9458 +116
+ Misses 1487 1476 -11
- Partials 310 313 +3
Flags with carried forward coverage won't be shown. Click here to find out more.
|
There was a problem hiding this comment.
Thanks for the follow-up. In principle this looks very good, some comments:
- shouldn't we also check for proper exported reference interfaces like here?
- in principle I'm fine with the change from RealtimeBox to RealtimeBuffer, but this soon might be changed to the lock-free queue. Is there a reason to change this in this PR? If not, I'd ask to remove the change because it is out-of-scope here.
|
To be honest I did not know the full implications of switching from a RealtimeBox to RealtimeBuffer, but I chose the change because every other example of chainable interface (eg. mecanum, passthrough, chainable-diff-drive) used it. I figured it probably had a reason for being used, but at the very least using the RealtimeBuffer would provide consistency across the chainable controllers. It seemed like the RealtimeBuffer was designed specifically for use-cases like the chainable interface, where the reference_interfaces could either be safely updated at a realtime rate or more slowly through the publisher, whereas the RealtimeBox would simply make a best-effort attempt at getting and setting the variables (which may in fact be sufficient for this use case) but didn't explicitly account for realtime vs non-realtime. Let me know if you would like me to proceed with changing RealtimeBuffer back to RealtimeBox, and I will do it. I agree that there should be a check for properly exported reference interfaces. I will imitate the mecanum drive when_controller_configured_expect_properly_exported_interfaces test. I will try to do that later today. |
with chained mode tests + export reference interfaces test
arthurlovekin
force-pushed
the
diff-drive-chainable-1457
branch
from
a353628 to
52ee68a
Compare
|
I just added a test for export_reference_interfaces(). |
christophfroehlich
changed the title
There was a problem hiding this comment.
OK I see your point regarding the RT buffer (I also think that the box was not the right thing to use). Thanks for adding the additional test.
I tested this version with the demos in the non-chained mode, and it still works like expected.
For future PRs: It is not desired to squash the commits (we do that at the time of the merge in the base branch), it just takes more time to review because I can't see now what has changed since the time of my last review.
|
Apologies, in the future I will not squash commits while the PR is in progress. The changes I made were to add the test |
you are right, I haven't realized that button before. You never stop learning ;) |
|
This pull request is in conflict. Could you fix it @arthurlovekin? |
|
Is there a way that I can fix the Rolling ABI compatibility check? It seems to be broken because of the change from ControllerInterface to ChainableControllerInterface and I see no way around that. Side question: I see in the documentation that there is a Working Group meeting every second Wednesday, but I cannot find the link to join. Would it be possible for me to attend? |
Dear @arthurlovekin the announcement, in ROS discourse, for tomorrow's meeting group is here: https://discourse.ros.org/t/ros2-control-working-group-meeting-15th-january-2025/41528 To join I copied paste the invitation in the announcement: |
| if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) | ||
| { | ||
| if (!is_halted) | ||
| { | ||
| halt(); | ||
| is_halted = true; | ||
| } | ||
| return controller_interface::return_type::OK; | ||
| } |
There was a problem hiding this comment.
Do we need this? The controller update cycle only happens in the active state
|
|
||
| previous_update_timestamp_ = time; | ||
| if (std::isnan(linear_command) || std::isnan(angular_command)) |
There was a problem hiding this comment.
| if (std::isnan(linear_command) || std::isnan(angular_command)) | |
| if (!std::isfinite(linear_command) || !std::isfinite(angular_command)) |
| reference_interfaces_[1] = 0.0; | ||
| } | ||
| else if ( | ||
| !std::isnan(command_msg_ptr->twist.linear.x) && !std::isnan(command_msg_ptr->twist.angular.z)) |
There was a problem hiding this comment.
| !std::isnan(command_msg_ptr->twist.linear.x) && !std::isnan(command_msg_ptr->twist.angular.z)) | |
| std::isfinite(command_msg_ptr->twist.linear.x) && std::isfinite(command_msg_ptr->twist.angular.z)) |
| if ( | ||
| cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || | ||
| current_time_diff < cmd_vel_timeout_) | ||
| { | ||
| received_velocity_msg_ptr_.writeFromNonRT(msg); | ||
| } | ||
| else | ||
| { | ||
| RCLCPP_WARN( | ||
| get_node()->get_logger(), | ||
| "Ignoring the received message (timestamp %.10f) because it is older than " | ||
| "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)", | ||
| rclcpp::Time(msg->header.stamp).seconds(), current_time_diff.seconds(), | ||
| cmd_vel_timeout_.seconds()); | ||
| } |
There was a problem hiding this comment.
This logic won't work if the header contains zeros
if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0))
You should modify the logic to be able to work with it
| @@ -498,6 +571,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( | |||
| controller_interface::CallbackReturn DiffDriveController::on_activate( | |||
| const rclcpp_lifecycle::State &) | |||
| { | |||
| // Set default value in command | |||
| reset_controller_reference_msg(*(received_velocity_msg_ptr_.readFromRT()), get_node()); | |||
There was a problem hiding this comment.
Personally, I don't like the idea of changing information of what is inside the buffer. The buffers should only be used to get but not modify it simultaneously.
| bool DiffDriveController::on_set_chained_mode(bool chained_mode) | ||
| { | ||
| // Always accept switch to/from chained mode | ||
| return true || chained_mode; | ||
| } |
There was a problem hiding this comment.
| bool DiffDriveController::on_set_chained_mode(bool chained_mode) | |
| { | |
| // Always accept switch to/from chained mode | |
| return true || chained_mode; | |
| } | |
| bool DiffDriveController::on_set_chained_mode(bool /*chained_mode*/) | |
| { | |
| // Always accept switch to/from chained mode | |
| return true; | |
| } |
| void reset_controller_reference_msg( | ||
| const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg, | ||
| const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node) | ||
| { | ||
| msg->header.stamp = node->now(); | ||
| msg->twist.linear.x = std::numeric_limits<double>::quiet_NaN(); | ||
| msg->twist.linear.y = std::numeric_limits<double>::quiet_NaN(); | ||
| msg->twist.linear.z = std::numeric_limits<double>::quiet_NaN(); | ||
| msg->twist.angular.x = std::numeric_limits<double>::quiet_NaN(); | ||
| msg->twist.angular.y = std::numeric_limits<double>::quiet_NaN(); | ||
| msg->twist.angular.z = std::numeric_limits<double>::quiet_NaN(); | ||
| } |
There was a problem hiding this comment.
Do we need to set the the velocities to NaN especially?, why not simply zero? This way you can just overload with the default constructor one.
If there is a specific reason, do you mind explaining here?
| previous_two_commands_.push({{0.0, 0.0}}); // needs zeros (not NaN) to catch early accelerations | ||
| previous_two_commands_.push({{0.0, 0.0}}); |
There was a problem hiding this comment.
I've a question, do we need to also reset this to zeros in the on_activate?
I'm not sure what would happen if you are deactivating and activating the controller and it might have the previous values still right?. May be I'm wrong here
Using dea3d3c as a reference, I made the diff_drive_controller into a ChainableControllerInterface. I used a RealtimeBuffer to handle the incoming velocity command messages, and I also added lifecycle checks and support for the speed_limiter that were not included in dea3d3c. I also added some tests that run the chainable controller in unchained mode, and I intend to test it in chained mode in one of my current projects (though I couldn't readily figure out how to make this into a test).