Added Swerve Drive Controller Package#1694
Conversation
christophfroehlich
left a comment
christophfroehlich
left a comment
There was a problem hiding this comment.
Thanks again for your contribution. Could you please fix the failing tests? A very quick first review below.
Do you mind add a brief section in the kinematics section of the docs, even in a new PR to get it merged faster? Thanks!
|
I'll make changes and push. |
…s.txt of swerve_drive_controller package Signed-off-by: nitin <nitinmaurya2606@gmail.com>
|
do you mind cherry-picking the changes to the docs in a separate PR? Splitting this up would help to reviewing things. |
I have raised a separate PR for changes in docs. PR #1712 |
Maverobot
left a comment
Maverobot
left a comment
There was a problem hiding this comment.
Hi there, thanks for your contribution! I've started reviewing the changes and have left a few comments. I will try to find time to work through it and will follow up once I'm done.
Co-authored-by: Zheng Qu <quzhengrobot@gmail.com>
|
This pull request is in conflict. Could you fix it @nitin2606? |
Signed-off-by: nitin <nitinmaurya2606@gmail.com>
…ix CI duplicate errors Signed-off-by: nitin2606 <nitinmaurya2606@gmail.com>
6567bcb to
c75660f
Compare
|
@nitin2606 Please don't do force pushes if a PR is already under review. It will complicate re-reviews a lot. And we will squash the commits at the time of merging a PR anyways. |
Thanks for the clarification I’ll keep the history stable from here and only merge upstream changes if needed. Thanks for the guidance. |
|
@christophfroehlich, could you please help me with the failing CI checks? |
Failures of rolling binary build and windows build are not related. Check Docs is failing because of:
You need to add it here https://github.com/ros-controls/ros2_controllers/blob/master/doc/controllers_index.rst#controllers-for-wheeled-mobile-robots |
Got it |
Signed-off-by: nitin2606 <nitinmaurya2606@gmail.com>
|
@christophfroehlich I added swerve_drive_controller to controllers_index.rst (see https://github.com/nitin2606/ros2_controllers/blob/feature/swerve-drive-controller/doc/controllers_index.rst |
|
This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete. |
|
This pull request is in conflict. Could you fix it @nitin2606? |
solonovamax
left a comment
solonovamax
left a comment
There was a problem hiding this comment.
just sort of skimmed through this on my phone (that's why each comment is only on a single line, sorry about that) and had some random comments
not super familiar with how things are done in this codebase however, so take some things with a grain of salt
| auto logger = get_node()->get_logger(); | ||
|
|
||
| wheel_handles_.resize(4); | ||
| for (std::size_t i = 0; i < 4; i++) |
There was a problem hiding this comment.
is on_activate really the best place for this?
looking at a handful of other controllers, it seems like this type of thing is generally put into on_configure
|
|
||
| for (std::size_t i = 0; i < wheel_handles_.size(); ++i) | ||
| { | ||
| if (!wheel_handles_[i]) |
There was a problem hiding this comment.
is there a reason this (the validation part, not the resetting part) isn't in the same for loop that creates the wheel handles?
| OdometryState odometry_; | ||
|
|
||
| // Topic Subscription | ||
| bool subscriber_is_active_ = false; |
There was a problem hiding this comment.
it's not obvious to me why this is necessary?
other controllers seem to just blindly update the stored command value from the topic, even if deactivated, as there's not really any harm in that
| rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; | ||
|
|
||
| // Timeout to consider cmd_vel commands old | ||
| std::chrono::milliseconds cmd_vel_timeout_{500}; |
There was a problem hiding this comment.
shouldn't this be an rclcpp::Duration?
| Params params_; | ||
|
|
||
| SwerveDriveKinematics swerveDriveKinematics_; | ||
| std::queue<TwistStamped> previous_commands_; // last two commands |
There was a problem hiding this comment.
seems to be unused?
regardless, storing a queue for two elements probably isn't the best idea
you could just have two fields with pointers and then swap them before overwriting the newer one, that way you avoid additional allocations
|
|
||
| protected: | ||
| // Handles for four wheels and their axles | ||
| std::vector<std::optional<Wheel>> wheel_handles_; |
There was a problem hiding this comment.
are these optional exclusively for the error handling?
imo it would be better to just make these not optional & have get_interface_object() return an optional, then in on_configure/on_activate if they cannot be found just erroring out
it avoids an necessary pointer dereference during normal operation (because optional is just a wrapper around a raw pointer + nullptr)
it also avoids a bunch of otherwise unecessary has_value() calls, because those can never happen.
|
|
||
| if (wheel_radius <= 0.0) | ||
| { | ||
| std::cerr << "invalid wheel_radius <= 0.0\n"; |
There was a problem hiding this comment.
should use the normal rclcpp logging
| if (wheel_radius <= 0.0) | ||
| { | ||
| std::cerr << "invalid wheel_radius <= 0.0\n"; | ||
| // fallthrough: compute but set angular velocities to 0 to avoid div-by-zero |
There was a problem hiding this comment.
wouldn't it be better to just completely error out?
|
|
||
| wheel_commands[i].drive_velocity = linear_speed; | ||
|
|
||
| if (wheel_radius > 0.0) |
There was a problem hiding this comment.
probably better to use a ternary here
| double vx_sum = 0.0, vy_sum = 0.0, wz_sum = 0.0; | ||
| for (std::size_t i = 0; i < 4; i++) | ||
| { | ||
| double vx = wheel_velocities[i] * std::cos(steering_angles[i]); |
There was a problem hiding this comment.
unsure if this is a GNU extension or not, but in math.h there's sincos which calculates the sine & cosine values simultaneously (though, the compiler might also optimize this for you, unsure)
10 participants
Description
This PR introduces the swerve_drive_controller, a new controller for swerve drive robots with four independently steerable wheels, enabling omnidirectional motion in ROS 2. It complements controllers like diff_drive_controller by supporting advanced mobile robot platforms.
Features
Supports geometry_msgs/msg/Twist or TwistStamped velocity inputs (x, y linear; z angular).
Publishes raw nav_msgs/msg/Odometry for user-defined post-processing.
Publishes /tf transforms (optional, if enable_odom_tf=true).
Configurable via YAML for wheel geometry and kinematic constraints.
Changes
Added swerve_drive_controller package (src/, include/, test/).
Exported as a pluginlib plugin (swerve_drive_controller_plugin.xml).
Updated ros2_controllers CMakeLists.txt and package.xml.
Included gtest tests (test/test_swerve_drive_controller.cpp) and config (test/config/test_swerve_drive_controller.yaml).