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

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

Codecov Report

❌ Patch coverage is 83.21918% with 98 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.15%. Comparing base (09f2dd6) to head (c670b2e).

Files with missing lines	Patch %	Lines
...e_drive_controller/src/swerve_drive_controller.cpp	74.44%	47 Missing and 22 partials ⚠️
...e_controller/test/test_swerve_drive_controller.hpp	84.68%	9 Missing and 8 partials ⚠️
...werve_drive_controller/swerve_drive_controller.hpp	76.00%	4 Missing and 2 partials ⚠️
...e_controller/test/test_swerve_drive_controller.cpp	95.41%	6 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1694      +/-   ##
==========================================
- Coverage   85.22%   85.15%   -0.08%     
==========================================
  Files         142      148       +6     
  Lines       13735    14318     +583     
  Branches     1191     1261      +70     
==========================================
+ Hits        11706    12192     +486     
- Misses       1632     1698      +66     
- Partials      397      428      +31

Flag	Coverage Δ
unittests	`85.15% <83.21%> (-0.08%)`	⬇️

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

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

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

christophfroehlich · 2025-08-18T06:20:17Z

swerve_drive_controller/test/test_swerve_drive_controller.hpp

+#ifndef TEST_SWERVE_DRIVE_CONTROLLER_HPP_
+#define TEST_SWERVE_DRIVE_CONTROLLER_HPP_
+
+#include <gtest/gtest.h>


we use gmock instead

Suggested change

#include <gtest/gtest.h>

#include <gmock/gmock.h>

christophfroehlich · 2025-08-18T06:20:40Z

swerve_drive_controller/CMakeLists.txt

+if(BUILD_TESTING)
+  find_package(controller_manager REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)


Suggested change

find_package(ament_cmake_gtest REQUIRED)

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

swerve_drive_controller/CMakeLists.txt

+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gtest(test_swerve_drive_controller


Suggested change

ament_add_gtest(test_swerve_drive_controller

ament_add_gmock(test_swerve_drive_controller

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

wsxq2

I find a problem.

wsxq2 · 2025-08-21T09:23:25Z

swerve_drive_controller/src/swerve_drive_controller.cpp

+  double x_offset = wheel_params_.x_offset;
+  double y_offset = wheel_params_.y_offset;
+  double radius = wheel_params_.radius;


Hi, these three variables are not being used, but they are useful. The x_offset and y_offset can be used to calculate wheel positions. I see that in your code the wheel positions are hardcoded, which doesn't seem reasonable.

Also, the radius can be used to calculate the wheel's angular velocity. I notice you're directly sending linear velocity to the hardware component, which also doesn't seem reasonable, since wheel speeds are typically expressed as angular velocities.

mergify · 2025-08-25T01:59:03Z

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.

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

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

wsxq2 suggested changes Aug 25, 2025

View reviewed changes

nitin2606 and others added 2 commits August 30, 2025 04:20

Modified Publisher Api, Test Files

87ea4e0

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

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

c670b2e

	ament_add_gtest(test_swerve_drive_controller
	ament_add_gmock(test_swerve_drive_controller

	::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!

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!

christophfroehlich Aug 18, 2025 • 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!

codecov bot commented Jul 22, 2025 •

edited

Loading

Amronos left a comment •

edited

Loading

christophfroehlich left a comment •

edited

Loading

christophfroehlich Aug 18, 2025 •

edited

Loading

nitin2606 commented Aug 29, 2025 •

edited

Loading