Added Swerve Drive Controller Package #1694

nitin2606 · 2025-05-21T11:17:38Z

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).

christophfroehlich

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!

swerve_drive_controller/CMakeLists.txt

swerve_drive_controller/LICENSE

swerve_drive_controller/doc/userdoc.rst

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp

swerve_drive_controller/package.xml

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/src/swerve_drive_kinematics.cpp

swerve_drive_controller/test/test_swerve_drive_controller.hpp

swerve_drive_controller/test/test_swerve_drive_controller.cpp

nitin2606 · 2025-05-22T05:50:46Z

I'll make changes and push.

…s.txt of swerve_drive_controller package Signed-off-by: nitin <[email protected]>

christophfroehlich · 2025-05-27T20:33:06Z

do you mind cherry-picking the changes to the docs in a separate PR? Splitting this up would help to reviewing things.

nitin2606 · 2025-05-27T22:26:08Z

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

doc/controllers_index.rst

Maverobot

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.

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

swerve_drive_controller/src/swerve_drive_kinematics.cpp

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

doc/mobile_robot_kinematics.rst

Co-authored-by: Zheng Qu <[email protected]>

mergify · 2025-06-24T17:03:08Z

This pull request is in conflict. Could you fix it @nitin2606?

Signed-off-by: nitin <[email protected]>

nitin2606 · 2025-08-29T23:03:55Z

@christophfroehlich, could you kindly review the changes in test_swerve_drive_controller.cpp line no 29. While the code appears to be correct, it is still failing the pre-commit checks. I would also greatly appreciate it if you could review the other changes you suggested.

Narukara · 2025-09-26T02:29:58Z

Thank you for all your work! I'm trying to test this controller locally with the Gazebo simulator. I'm searching for information on how to compile and use a new controller locally... 😃 If anyone could point me to a suitable tutorial, that would be helpful. I'll also try this myself.

nitin2606 · 2025-09-26T06:28:38Z

Thank you for all your work! I'm trying to test this controller locally with the Gazebo simulator. I'm searching for information on how to compile and use a new controller locally... 😃 If anyone could point me to a suitable tutorial, that would be helpful. I'll also try this myself.

I have made a local setup to test this controller, if you want I can share the github link.

Narukara · 2025-09-26T06:36:43Z

I have made a local setup to test this controller, if you want I can share the github link.

Yes, please! That would be extremely helpful. Thank you for offering to share it. 😄

nitin2606 · 2025-09-26T06:53:18Z

I have made a local setup to test this controller, if you want I can share the github link.

Yes, please! That would be extremely helpful. Thank you for offering to share it. 😄

https://github.com/nitin2606/Swerve_Drive.git

wsxq2 · 2025-09-28T01:32:33Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  {-0.1, 0.175},   // front left
+  {0.1, 0.175},    // front right
+  {-0.1, -0.175},  // rear left
+  {0.1, -0.175}    // rear right


Thank you for all your work! I really like this work. I note here you use a fixed wheel_positions_, I feel this is unreasonable. I have made similar suggestions before, but I did not receive a response from you. What are your thoughts on this? @nitin2606 .

This is my first time using the review function of GitHub. If there is anything wrong, please correct me.

Yes you are right, wheel positions must be taken via parameters. Sorry for the late response.
I'll make necessary changes and push.

I've noticed another issue. According to ROS REP 103, the forward direction of a vehicle should align with the X-axis. That means the positions of the four wheels in the coordinate frame should be:

^y Rear Left | Front Left ------------+-------------->x Rear Right | Front Right

This is also how the Ackermann vehicle model is designed in this URDF file. However, the current placement here does not seem to comply with this, which could potentially have some implications for the code.

The kinematics code already follows REP-103 (+X forward, +Y left). The issue is just mislabeled wheel positions in the array comments; once reordered or relabeled consistently (FL=+X,+Y; FR=+X,–Y; RR=–X,–Y; RL=–X,+Y), everything aligns correctly.

Narukara · 2025-09-30T07:40:11Z

There's another issue: the userdoc.rst mentions that the use_stamped_vel option can be used to configure the controller to accept either TwistStamped or Twist messages. However, this does not appear to be implemented in the code — the controller always uses TwistStamped. 🤔

nitin2606 · 2025-09-30T08:17:45Z

There's another issue: the userdoc.rst mentions that the use_stamped_vel option can be used to configure the controller to accept either TwistStamped or Twist messages. However, this does not appear to be implemented in the code — the controller always uses TwistStamped. 🤔

userdoc.rst is not updated. All the controllers of ros2_control have switched to TwistStamped only.

computed from wheelbase, trackwidth, offset_x, and offset_y. This improves flexibility and reduces duplication when changing robot dimensions. Signed-off-by: nitin <[email protected]>

Narukara

I'm not sure if this is the right place to handle the wheel radius. I tested the modified controller using gazebo, and both the controls and odometry seem to work fine. By the way, I used the humble version, so I made some necessary changes.

Narukara · 2025-10-07T08:49:51Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  auto logger = get_node()->get_logger();
+
+  RCLCPP_INFO(logger, "Updated Kinematics");


Suggested change

auto logger = get_node()->get_logger();

RCLCPP_INFO(logger, "Updated Kinematics");

The controller prints a lot of this when it runs, which is a bit annoying.

Narukara · 2025-10-07T08:50:26Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+        wheel_joint_names[i]);
+    }
+    axle_handles_[i]->set_position(wheel_command[i].steering_angle);
+    wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity);


Suggested change

wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity);

wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity / params_.wheel_radius);

Solved in latest commit

Narukara · 2025-10-07T08:50:59Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+    }
+    else
+    {
+      velocity_array[i] = wheel_handles_[i]->get_feedback();


Suggested change

velocity_array[i] = wheel_handles_[i]->get_feedback();

velocity_array[i] = wheel_handles_[i]->get_feedback() * params_.wheel_radius;

Solved in latest commit

Narukara · 2025-10-07T09:04:01Z

Another small issue: I found that when I stop sending speed commands, the controller automatically straightens all the wheels. Maybe it would be more reasonable to keep each wheel at its current steering angle?

- Updated kinematics to compute angular velocity for wheels instead of linear velocity for more accurate motion control. - Enhanced controller to retain the last steering angle when stopped, preventing the wheels from snapping back to zero orientation.

nitin2606 · 2025-10-07T22:24:24Z

Another small issue: I found that when I stop sending speed commands, the controller automatically straightens all the wheels. Maybe it would be more reasonable to keep each wheel at its current steering angle?

please check latest commit

Narukara · 2025-10-08T07:25:33Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  if (realtime_odometry_publisher_)
+  {
+    auto & odometry_message = realtime_odometry_publisher_->msg_;
+    odometry_message.header.stamp = time;
+    odometry_message.pose.pose.position.x = odometry_.x;
+    odometry_message.pose.pose.position.y = odometry_.y;
+    odometry_message.pose.pose.orientation.x = orientation.x();
+    odometry_message.pose.pose.orientation.y = orientation.y();
+    odometry_message.pose.pose.orientation.z = orientation.z();
+    odometry_message.pose.pose.orientation.w = orientation.w();
+    realtime_odometry_publisher_->tryPublish(odometry_message);
+  }


The odometry message published here does not contain twist information (twist.linear.x, twist.linear.y, twist.angular.z). I have confirmed this issue in the Gazebo test.

Narukara

add odometry twist

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp

swerve_drive_controller/src/swerve_drive_controller.cpp

swerve_drive_controller/src/swerve_drive_kinematics.cpp

…_drive_kinematics.hpp Co-authored-by: Narukara <[email protected]>

Co-authored-by: Narukara <[email protected]>

Signed-off-by: nitin <[email protected]>

Narukara · 2025-10-14T07:28:45Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.

2025.10.14.15.21.35.webm

nitin2606 · 2025-10-14T07:40:53Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.

2025.10.14.15.21.35.webm

This appears to be a kinematics issue — when the robot is commanded to move backward, the wheels shouldn’t rotate; they should simply translate backward.

christophfroehlich · 2025-10-14T14:57:32Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.
2025.10.14.15.21.35.webm

This appears to be a kinematics issue — when the robot is commanded to move backward, the wheels shouldn’t rotate; they should simply translate backward.

This is a "feature" of the solution of the inverse kinematics not being unique. But this could be handled to have a best effort behavior. Having a look a the video, there should also be some master-slave behavior of the steering and speed command of the wheels be employed, see the reduce_wheel_speed_until_steering_reached parameter here:

https://github.com/ros-controls/ros2_controllers/blob/d7a699fd82acd90a54b8a28f809d29d705968ff6/steering_controllers_library/src/steering_odometry.cpp#L263C21-L284

Signed-off-by: nitin <[email protected]>

nitin2606 · 2025-10-14T21:58:26Z

I've found another tricky problem. When I send the command tw.linear.x = -0.5, I expect the car to move backward. But what actually happens is that it first rotates the wheels 180 degrees and then moves forward, as shown in this video.
2025.10.14.15.21.35.webm

This appears to be a kinematics issue — when the robot is commanded to move backward, the wheels shouldn’t rotate; they should simply translate backward.

This is a "feature" of the solution of the inverse kinematics not being unique. But this could be handled to have a best effort behavior. Having a look a the video, there should also be some master-slave behavior of the steering and speed command of the wheels be employed, see the reduce_wheel_speed_until_steering_reached parameter here:

https://github.com/ros-controls/ros2_controllers/blob/d7a699fd82acd90a54b8a28f809d29d705968ff6/steering_controllers_library/src/steering_odometry.cpp#L263C21-L284

Please check the latest commit, I have tried to fix the issue

nitin2606 and others added 2 commits May 21, 2025 16:34

Add swerve_drive_controller package with tests and documentation

51c5c08

Merge branch 'ros-controls:master' into feature/swerve-drive-controller

2cb3f0f

github-actions bot requested review from Maverobot, anfemosa, bijoua29, bmagyar, harderthan, mhubii and saikishor May 21, 2025 11:17

nitin2606 mentioned this pull request May 21, 2025

Add Swerve Drive Controller #1692

Closed

christophfroehlich requested changes May 21, 2025

View reviewed changes

christophfroehlich linked an issue May 21, 2025 that may be closed by this pull request

Add a controller for afour wheel independent steering / drive mobile base, aka a swerve drive #1425

Open

nitin2606 and others added 2 commits May 25, 2025 22:21

Merge branch 'ros-controls:master' into feature/swerve-drive-controller

89c269d

Added Swerve Drive In mobile_robot_kinematics.rst and fixed CMakeList…

a6b6ac7

…s.txt of swerve_drive_controller package Signed-off-by: nitin <[email protected]>

nitin2606 requested a review from christophfroehlich May 27, 2025 20:01

Merge branch 'ros-controls:master' into feature/swerve-drive-controller

a146ff8

nitin2606 mentioned this pull request May 27, 2025

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst #1712

Merged

Merge branch 'master' into feature/swerve-drive-controller

369bfd3

bmagyar reviewed Jun 5, 2025

View reviewed changes

doc/controllers_index.rst Outdated Show resolved Hide resolved

Apply suggestions from code review

647b161

Maverobot reviewed Jun 5, 2025

View reviewed changes

doc/mobile_robot_kinematics.rst Show resolved Hide resolved

This was referenced Jun 23, 2025

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst (backport #1712) #1770

Merged

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst (backport #1712) #1771

Merged

Update swerve_drive_controller/src/swerve_drive_controller.cpp

8b72f06

Co-authored-by: Zheng Qu <[email protected]>

Added Swerve Drive Kinematics in mobile_robot_kinematics.rst

ee8d704

Signed-off-by: nitin <[email protected]>

Merge branch 'master' into feature/swerve-drive-controller

66fe918

wsxq2 suggested changes Sep 28, 2025

View reviewed changes

Instead of hardcoding wheel positions, wheel coordinates are now

bc5aa20

computed from wheelbase, trackwidth, offset_x, and offset_y. This improves flexibility and reduces duplication when changing robot dimensions. Signed-off-by: nitin <[email protected]>

nitin2606 requested a review from christophfroehlich September 30, 2025 18:52

Narukara reviewed Oct 7, 2025

View reviewed changes

Merge branch 'master' into feature/swerve-drive-controller

e67464d

Narukara reviewed Oct 8, 2025

View reviewed changes

Narukara suggested changes Oct 10, 2025

View reviewed changes

nitin2606 and others added 6 commits October 10, 2025 13:36

Update swerve_drive_controller/include/swerve_drive_controller/swerve…

a56aa93

…_drive_kinematics.hpp Co-authored-by: Narukara <[email protected]>

Update swerve_drive_controller/src/swerve_drive_controller.cpp

df62ca8

Co-authored-by: Narukara <[email protected]>

Update swerve_drive_controller/src/swerve_drive_kinematics.cpp

cf0a3d0

Co-authored-by: Narukara <[email protected]>

Update swerve_drive_controller/src/swerve_drive_kinematics.cpp

631e97c

Co-authored-by: Narukara <[email protected]>

Made swerve_drive_controller chainable

37a2ece

Signed-off-by: nitin <[email protected]>

Suppressed false syntax error

4674fa7

Signed-off-by: nitin <[email protected]>

nitin2606 requested a review from Narukara October 13, 2025 20:41

choose minimal steering rotation and reverse drive when needed

c5ce36d

Signed-off-by: nitin <[email protected]>

		auto logger = get_node()->get_logger();

		RCLCPP_INFO(logger, "Updated Kinematics");

	wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity);
	wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity / params_.wheel_radius);

	velocity_array[i] = wheel_handles_[i]->get_feedback();
	velocity_array[i] = wheel_handles_[i]->get_feedback() * params_.wheel_radius;

Added Swerve Drive Controller Package #1694

Are you sure you want to change the base?

Added Swerve Drive Controller Package #1694

Conversation

nitin2606 commented May 21, 2025

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

nitin2606 commented May 22, 2025

Uh oh!

christophfroehlich commented May 27, 2025

Uh oh!

nitin2606 commented May 27, 2025

Uh oh!

Uh oh!

Maverobot left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Jun 24, 2025

Uh oh!

nitin2606 commented Aug 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Narukara commented Sep 26, 2025

Uh oh!

nitin2606 commented Sep 26, 2025

Uh oh!

Narukara commented Sep 26, 2025

Uh oh!

nitin2606 commented Sep 26, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Narukara commented Sep 30, 2025

Uh oh!

nitin2606 commented Sep 30, 2025

Uh oh!

Narukara left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Narukara commented Oct 7, 2025

Uh oh!

nitin2606 commented Oct 7, 2025

Uh oh!

nitin2606 commented Aug 29, 2025 •

edited

Loading