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]>

mergify · 2025-07-22T19:54:05Z

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

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

codecov · 2025-07-22T21:38:31Z

Codecov Report

❌ Patch coverage is 18.28299% with 514 lines in your changes missing coverage. Please review.
✅ Project coverage is 82.89%. Comparing base (a01fd0b) to head (71dd2bc).

Files with missing lines	Patch %	Lines
...e_drive_controller/src/swerve_drive_controller.cpp	4.07%	305 Missing and 1 partial ⚠️
...e_controller/test/test_swerve_drive_controller.cpp	33.59%	154 Missing and 16 partials ⚠️
...e_drive_controller/src/swerve_drive_kinematics.cpp	7.31%	38 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1694      +/-   ##
==========================================
- Coverage   85.98%   82.89%   -3.10%     
==========================================
  Files         129      134       +5     
  Lines       13112    13740     +628     
  Branches     1144     1216      +72     
==========================================
+ Hits        11275    11390     +115     
- Misses       1469     1966     +497     
- Partials      368      384      +16

Flag	Coverage Δ
unittests	`82.89% <18.28%> (-3.10%)`	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...werve_drive_controller/swerve_drive_controller.hpp	`100.00% <100.00%> (ø)`
...troller/test/test_load_swerve_drive_controller.cpp	`100.00% <100.00%> (ø)`
...e_drive_controller/src/swerve_drive_kinematics.cpp	`7.31% <7.31%> (ø)`
...e_controller/test/test_swerve_drive_controller.cpp	`33.59% <33.59%> (ø)`
...e_drive_controller/src/swerve_drive_controller.cpp	`4.07% <4.07%> (ø)`

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Amronos

I have done a rough review right now and haven't gone through the main source code properly.
Aside from my other review comments, I noticed the following:

You have the GPL parameters yaml, but aren't using GPL properly anywhere in your source code.
Why not make this controller a chained controller?
Are you using the new APIs here? (Look at #1566 and #1830)

doc/migration.rst

doc/release_notes.rst

swerve_drive_controller/src/swerve_drive_controller_parameter.yaml

swerve_drive_controller/LICENSE

swerve_drive_controller/package.xml

swerve_drive_controller/test/config/test_swerve_drive_controller.yaml

swerve_drive_controller/src/swerve_drive_controller_parameter.yaml

doc/release_notes.rst

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

Amronos

Please add my suggestions to batch on the files changed tab and commit from there. This will make it easier to review.

doc/migration.rst

doc/release_notes.rst

swerve_drive_controller/package.xml

Co-authored-by: Aarav Gupta <[email protected]>

nitin2606 · 2025-08-08T09:18:26Z

@Amronos @christophfroehlich Could you please review when you get a chance?

christophfroehlich

To emphasize the last commit above: Please either use the github UI to accept proposed changes, or leave the conversations open so that the reviewing person can immediately check if they have been addressed or a new comment was added. Only the reviewer should manually close the conversations. This improves effectivity of the next review round.

Please fix the failing linter jobs: Install pre-commit via pre-commit install for the next commits; the current issues can be fixed with pre-commit run --all from the repo.

swerve_drive_controller/doc/userdoc.rst

Co-authored-by: Christoph Fröhlich <[email protected]>

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

nitin2606 · 2025-08-17T22:54:38Z

To emphasize the last commit above: Please either use the github UI to accept proposed changes, or leave the conversations open so that the reviewing person can immediately check if they have been addressed or a new comment was added. Only the reviewer should manually close the conversations. This improves effectivity of the next review round.

Please fix the failing linter jobs: Install pre-commit via pre-commit install for the next commits; the current issues can be fixed with pre-commit run --all from the repo.

I have attempted to resolve the linter issues, but I am still encountering a persistent error reported by the pre-commit check at line 152 of the file: https://github.com/nitin2606/ros2_controllers/blob/feature/swerve-drive-controller/swerve_drive_controller/test/test_swerve_drive_controller.cpp
Could you kindly review it and help me identify the underlying issue?

christophfroehlich

A first review round, haven't gone over the kinematics implementation yet.

swerve_drive_controller/test/test_swerve_drive_controller.hpp

swerve_drive_controller/CMakeLists.txt

christophfroehlich · 2025-08-18T06:21:38Z

swerve_drive_controller/test/test_swerve_drive_controller.cpp

+  if (!init_successful_)
+  {
+    GTEST_SKIP() << "Skipping due to on_init failure";
+  }


We use gmock in our projects, please use a pattern like this

Suggested change

if (!init_successful_)

{

GTEST_SKIP() << "Skipping due to on_init failure";

}

ASSERT_TRUE(init_successful_) << "Skipping due to on_init failure";

Got it I'll make necessary changes

christophfroehlich · 2025-08-18T06:21:58Z

swerve_drive_controller/test/test_swerve_drive_controller.cpp

+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);


Suggested change

::testing::InitGoogleTest(&argc, argv);

::testing::InitGoogleMock(&argc, argv);

christophfroehlich · 2025-08-18T06:25:24Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+    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_->unlockAndPublish();


Please update the publisher API like in #1830

christophfroehlich · 2025-08-18T06:27:26Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  auto node = get_node();
+  if (!node)
+  {
+    RCLCPP_ERROR(rclcpp::get_logger("SwerveController"), "Node is null in on_init");


Why are you always creating a new logger? Please use

Suggested change

RCLCPP_ERROR(rclcpp::get_logger("SwerveController"), "Node is null in on_init");

RCLCPP_ERROR(node->get_logger(), "Node is null in on_init");

instead.

Applies to all occurrences

christophfroehlich · 2025-08-18T06:32:42Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  static_assert(
+    !std::is_const_v<std::remove_reference_t<decltype(*command_handle)>>,
+    "Command handle is const!");
+  return std::make_unique<T>(std::ref(*command_handle), std::ref(*state_handle), name);


Can you please elaborate why you used this pattern? When should this ever be constant?
In other controllers, we use std::reference_wrapper instead of a unique ptr

ros2_controllers/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

Line 87 in d1e4bc3

std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;

Actually this change was suggested by one of the reviewers.

christophfroehlich · 2025-08-18T06:35:35Z

swerve_drive_controller/src/swerve_drive_controller_parameter.yaml

+  publishe_rate: {
+    type: double,
+    default_value: 50.0, # Hz
+    description: "Publishing rate (Hz) of the odometry and TF messages.",
+    read_only: true
+  }


Please remove the publish rate. We decided to remove this from all controllers, they should publish with the update rate of the controller.
#473

christophfroehlich · 2025-08-18T06:36:52Z

swerve_drive_controller/src/swerve_drive_kinematics.cpp

+  return odometry_;
+}
+
+double SwerveDriveKinematics::normalize_angle(double angle)


Please use angles::normalize_angle from angles/angles.h instead of adding a new method.

I'll make all the mentioned changes

Co-authored-by: Christoph Fröhlich <[email protected]>

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 anfemosa, bijoua29, bmagyar, harderthan, Maverobot, 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 Outdated 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]>

bmagyar and others added 2 commits July 22, 2025 17:46

Fix format

42c3328

Mofied Joint And Axle Name Variables

879ecb2

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

nitin2606 and others added 2 commits July 23, 2025 01:25

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

c74b04a

Modified CMakeLists.txt

ce247f1

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

nitin2606 added 2 commits July 25, 2025 00:54

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

8eea7b1

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

5df6bf3

Amronos suggested changes Jul 26, 2025

View reviewed changes

nitin2606 and others added 5 commits July 27, 2025 19:43

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

4a5afae

Mentioned swerve_driver_controller in release_notes.rst

aa2899d

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

Made use of generate parameters library

67adf94

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

Removed unwanted merge

3a0714e

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

Made parameters read only

1b2a6b1

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

Amronos suggested changes Jul 28, 2025

View reviewed changes

doc/migration.rst Outdated Show resolved Hide resolved

doc/release_notes.rst Outdated Show resolved Hide resolved

swerve_drive_controller/package.xml Outdated Show resolved Hide resolved

nitin2606 and others added 3 commits July 28, 2025 15:07

Update doc/migration.rst

88ed2ba

Co-authored-by: Aarav Gupta <[email protected]>

Update doc/release_notes.rst

e9d72bb

Co-authored-by: Aarav Gupta <[email protected]>

Update swerve_drive_controller/package.xml

d4b5d8e

Co-authored-by: Aarav Gupta <[email protected]>

nitin2606 requested a review from Amronos July 28, 2025 09:38

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

71dd2bc

christophfroehlich requested changes Aug 17, 2025

View reviewed changes

christophfroehlich reviewed Aug 17, 2025

View reviewed changes

swerve_drive_controller/doc/userdoc.rst Outdated Show resolved Hide resolved

nitin2606 and others added 2 commits August 18, 2025 00:46

Update swerve_drive_controller/doc/userdoc.rst

3288507

Co-authored-by: Christoph Fröhlich <[email protected]>

Fixed Failing Linter Jobs

3daf814

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

christophfroehlich requested changes Aug 18, 2025

View reviewed changes

nitin2606 and others added 3 commits August 19, 2025 02:45

Update swerve_drive_controller/test/test_swerve_drive_controller.hpp

8c730a7

Co-authored-by: Christoph Fröhlich <[email protected]>

Update swerve_drive_controller/CMakeLists.txt

a7be47f

Co-authored-by: Christoph Fröhlich <[email protected]>

Update swerve_drive_controller/CMakeLists.txt

16d3fa9

Co-authored-by: Christoph Fröhlich <[email protected]>

	::testing::InitGoogleTest(&argc, argv);
	::testing::InitGoogleMock(&argc, argv);

	RCLCPP_ERROR(rclcpp::get_logger("SwerveController"), "Node is null in on_init");
	RCLCPP_ERROR(node->get_logger(), "Node is null in on_init");

Added Swerve Drive Controller Package #1694

Are you sure you want to change the base?

Added Swerve Drive Controller Package #1694

Uh oh!

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!

mergify bot commented Jul 22, 2025

Uh oh!

codecov bot commented Jul 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Amronos left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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!

Amronos left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

nitin2606 commented Aug 8, 2025

Uh oh!

christophfroehlich left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

nitin2606 commented Aug 17, 2025

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

codecov bot commented Jul 22, 2025 •

edited

Loading

Amronos left a comment •

edited

Loading

christophfroehlich left a comment •

edited

Loading