Add AckermannDriveStamped control to steering library #1563

wittenator · 2025-03-01T11:00:39Z

This PR adds the option to use steering angle and linear velocity for controllers that inherit from the steering library.

To send us a pull request, please:

Fork the repository.
Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change.
Ensure local tests pass. (colcon test and pre-commit run (requires you to install pre-commit by pip3 install pre-commit)
Commit to your fork using clear commit messages.
Send a pull request, answering any default questions in the pull request interface.
Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.

wittenator · 2025-03-01T11:12:19Z

The tests on HEAD are currently broken, but I wanted to get this PR out in order to track progress.

christophfroehlich

why WIP?
Please fix the failing jobs, start with the pre-commit and clang job.
The tests on the master branch are green.

steering_controllers_library/src/steering_odometry.cpp

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

wittenator · 2025-04-11T20:33:58Z

@christophfroehlich I fixed all the pre-commit checks and the colcon test checks. I think this should be ready for a first review now.

wittenator · 2025-04-24T09:30:50Z

@christophfroehlich Since there is quite a bit of incoming movement in the steering controllers library and the odometry library. Are we holding off with this PR's addition until these parts stabilized again?

christophfroehlich · 2025-04-24T10:59:40Z

there is only one which i have on the list to be merged soon #836. I try to review yours within the next days

wittenator · 2025-04-24T12:16:25Z

No hurry, I need to fix the tests with the newest merge anyway. I'll ping you here, once I am done with this

wittenator · 2025-04-26T21:28:00Z

@christophfroehlich I fixed the tests. This should be ready now

codecov · 2025-04-26T22:17:05Z

Codecov Report

Attention: Patch coverage is 91.28440% with 19 lines in your changes missing coverage. Please review.

Project coverage is 84.94%. Comparing base (8c649a3) to head (b42d13a).
Report is 1 commits behind head on master.

Files with missing lines	Patch %	Lines
...llers_library/src/steering_controllers_library.cpp	76.08%	10 Missing and 1 partial ⚠️
...g_controller/src/ackermann_steering_controller.cpp	0.00%	2 Missing ⚠️
...ing_controller/src/bicycle_steering_controller.cpp	0.00%	2 Missing ⚠️
...st/test_steering_controllers_library_ackermann.hpp	97.22%	1 Missing and 1 partial ⚠️
...ng_controller/src/tricycle_steering_controller.cpp	0.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1563      +/-   ##
==========================================
+ Coverage   84.77%   84.94%   +0.17%     
==========================================
  Files         127      129       +2     
  Lines       12096    12277     +181     
  Branches     1036     1048      +12     
==========================================
+ Hits        10254    10429     +175     
- Misses       1503     1509       +6     
  Partials      339      339

Flag	Coverage Δ
unittests	`84.94% <91.28%> (+0.17%)`	⬆️

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

Files with missing lines	Coverage Δ
...roller/test/test_ackermann_steering_controller.cpp	`100.00% <100.00%> (ø)`
...ntroller/test/test_bicycle_steering_controller.cpp	`100.00% <100.00%> (ø)`
...steering_controllers_library/steering_odometry.hpp	`100.00% <ø> (ø)`
...ring_controllers_library/src/steering_odometry.cpp	`83.61% <100.00%> (+0.18%)`	⬆️
...library/test/test_steering_controllers_library.cpp	`100.00% <100.00%> (ø)`
...library/test/test_steering_controllers_library.hpp	`97.22% <100.00%> (ø)`
...st/test_steering_controllers_library_ackermann.cpp	`100.00% <100.00%> (ø)`
...ontrollers_library/test/test_steering_odometry.cpp	`100.00% <100.00%> (ø)`
...troller/test/test_tricycle_steering_controller.cpp	`100.00% <100.00%> (ø)`
...g_controller/src/ackermann_steering_controller.cpp	`48.97% <0.00%> (-1.03%)`	⬇️
... and 4 more

🚀 New features to boost your workflow:

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

mergify · 2025-04-27T07:57:39Z

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

Juliaj

The changes look good. A few suggestions for improvement: Could we update the relevant documentation to reflect this change? It would be valuable to add more test coverage for the new functionality - particularly in the three controllers modified by this PR.

Juliaj · 2025-04-28T16:32:35Z

steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml

+    front_wheel_track: 2.12321
+    rear_wheel_track: 1.76868
+    front_wheels_radius: 0.45
+    rear_wheels_radius: 0.45


The names of these parameters have been changed, see #1166. Can we update these accordingly ?

Juliaj · 2025-04-28T16:43:41Z

steering_controllers_library/test/steering_controllers_library_params.yaml

@@ -5,6 +5,7 @@ test_steering_controllers_library:
    open_loop: false
    velocity_rolling_window_size: 10
    position_feedback: false
+    twist_input: true


Being relatively new to ros2_control development, I'm not entirely familiar with the established practices to define params ... I wanted to share a thought on the parameter naming and type choice. The current bool parameter doesn't call out the supported input options. An enum-based parameter named input_type could be an alternative but I'm not sure about the complexity that these introduce.

afaik there is no enum-like parameter feature in rclcpp. The only thing we could use is a string array, with a one_of<> validator.
I don't have a strong opinion here, maybe other reviewers have.

Juliaj · 2025-04-28T16:45:47Z

steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp

@@ -0,0 +1,215 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)


nit, could we name this file as ..._library_ackerman_input.cpp ?

Juliaj · 2025-04-28T16:49:43Z

steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp

+  unsigned int velocity_rolling_window_size_ = 10;
+  bool position_feedback_ = false;
+  std::vector<std::string> rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
+  std::vector<std::string> front_wheels_names_ = {


Similar to comments above on the changes introduced by #1166, please help update these variable names.

christophfroehlich · 2025-04-28T20:21:14Z

steering_controllers_library/src/steering_controllers_library.yaml

+  twist_input: {
+    type: bool,
+    default_value: true,
+    description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.",


Suggested change

description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.",

description: "Choose whether a TwistStamped or a AckermannDriveStamped message is used as input.",

we don't allow unstamped messages from jazzy on, or am I missing something?

christophfroehlich

I have difficulties to follow your code with mixing phi and angular velocity, can you please clarify this for me? And some minor comments added to those of @Juliaj

christophfroehlich · 2025-04-28T20:26:07Z

steering_controllers_library/src/steering_controllers_library.yaml

@@ -167,3 +167,10 @@ steering_controllers_library:
    position_feedback is true then ``HW_IF_POSITION`` is taken as interface type",
    read_only: false,
  }
+
+  twist_input: {


should we rename it to use_twist_input? This would make the params_.twist_input code more descriptive.

christophfroehlich · 2025-04-28T20:28:51Z

steering_controllers_library/src/steering_controllers_library.yaml

+    type: bool,
+    default_value: true,
+    description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.",
+    read_only: false,


does it make sense to change this when the controller is running?

christophfroehlich · 2025-04-28T20:32:13Z

steering_controllers_library/src/steering_odometry.cpp

@@ -180,16 +182,18 @@ bool SteeringOdometry::update_from_velocity(

  double linear_velocity = get_linear_velocity_double_traction_axle(
    right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
-  const double angular_velocity = steer_pos_ * linear_velocity / wheel_base_;


oops, the std::tan was missing here?

christophfroehlich · 2025-04-28T20:41:49Z

steering_controllers_library/src/steering_odometry.cpp

@@ -252,7 +261,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
  }
 #endif
  // steering angle
-  phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
+  phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz) : omega_bz;


why is this phi=omega_bz for ackermann input?

christophfroehlich · 2025-04-28T20:44:41Z

steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp

+  std::vector<std::string> joint_names_ = {
+    rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]};
+
+  std::vector<std::string> rear_wheels_preceeding_names_ = {


Please fix the typos here, see #1655

christophfroehlich · 2025-04-28T20:45:14Z

steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp

+  std::array<double, 4> joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}};
+  std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};
+
+  std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};


does it make sense to call this variable joint_?

christophfroehlich · 2025-04-28T20:57:48Z

steering_controllers_library/src/steering_odometry.cpp

 {
  /// Save last linear and angular velocity:
  linear_ = v_bx;
-  angular_ = omega_bz;
+  angular_ = twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz);


there is no steering angle here?

wittenator added 8 commits March 1, 2025 08:50

Add additional tests

768ef15

Merge branch 'ros-controls:master' into master

e22df84

Remove twist_input from closed loop odometry calculation

36bc9d5

Merge branch 'ros-controls:master' into master

94eebd0

Add in missing test case

c0f103e

Merge branch 'master' of github.com:wittenator/ros2_controllers

0564ada

Fixed comment

eaebf09

Update on ackermann message

3e84edd

github-actions bot requested review from anfemosa, christophfroehlich, destogl, erickisos, mhubii, moriarty and rosterloh March 1, 2025 11:00

wittenator changed the title ~~Add AckermannDriveStamped control to steering library~~ WIP: Add AckermannDriveStamped control to steering library Mar 1, 2025

wittenator mentioned this pull request Mar 1, 2025

WIP: Add AckermannDriveStamped control to steering library #1171

Closed

6 tasks

wittenator and others added 3 commits March 3, 2025 14:03

Merge branch 'master' into master

4f14831

Merge branch 'master' into master

e20e63c

Merge branch 'master' into master

031ddc6

christophfroehlich requested changes Mar 19, 2025

View reviewed changes

steering_controllers_library/src/steering_odometry.cpp Outdated Show resolved Hide resolved

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp Outdated Show resolved Hide resolved

wittenator added 8 commits April 6, 2025 18:16

Merge branch 'master' into master

7564e34

Fix reference name

0c3ded5

Merge branch 'master' of github.com:wittenator/ros2_controllers

a15dc7f

Merge branch 'ros-controls:master' into master

66effcf

Fix typo

d204e7e

Merge branch 'master' of github.com:wittenator/ros2_controllers

948b67c

Fix tests

c612f3c

Fix exported interfaces tests

915e3bd

wittenator added 3 commits April 11, 2025 20:25

Add comment

b038514

Ran precommit hooks

b79d682

Fix all pre-commit checks

f9f2459

wittenator changed the title ~~WIP: Add AckermannDriveStamped control to steering library~~ Add AckermannDriveStamped control to steering library Apr 11, 2025

wittenator added 7 commits April 11, 2025 20:36

Update function description

3782090

Merge branch 'ros-controls:master' into master

aae242d

Fix missed readability issue

abbe4fd

Merge branch 'master' of github.com:wittenator/ros2_controllers

fc881c2

Switch from double to float for drive messages in tests

5a6e60e

Explicitly cast default parameters to float

ed3ef21

Merge branch 'ros-controls:master' into master

8b65ff9

wittenator requested a review from christophfroehlich April 17, 2025 07:36

Merge branch 'master' into master

976935a

wittenator mentioned this pull request Apr 24, 2025

Use Generic Steering Controller State Message in Steering Controllers Library #836

Closed

wittenator added 2 commits April 26, 2025 20:29

Fixed tests again

bb27729

Merge branch 'ros-controls:master' into master

b42d13a

wittenator added 2 commits April 27, 2025 10:38

Merge branch 'master' into master

c367db2

Merge branch 'master' into master

3436bb0

Juliaj reviewed Apr 28, 2025

View reviewed changes

christophfroehlich reviewed Apr 28, 2025

View reviewed changes

christophfroehlich requested changes Apr 28, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add AckermannDriveStamped control to steering library #1563

Add AckermannDriveStamped control to steering library #1563

wittenator commented Mar 1, 2025 •

edited

Loading

wittenator commented Mar 1, 2025 •

edited

Loading

christophfroehlich left a comment

wittenator commented Apr 11, 2025

wittenator commented Apr 24, 2025

christophfroehlich commented Apr 24, 2025

wittenator commented Apr 24, 2025

wittenator commented Apr 26, 2025 •

edited

Loading

codecov bot commented Apr 26, 2025 •

edited

Loading

mergify bot commented Apr 27, 2025

Juliaj left a comment

Juliaj Apr 28, 2025 •

edited

Loading

Juliaj Apr 28, 2025 •

edited

Loading

christophfroehlich Apr 28, 2025

Juliaj Apr 28, 2025

Juliaj Apr 28, 2025

christophfroehlich Apr 28, 2025

christophfroehlich left a comment

christophfroehlich Apr 28, 2025

christophfroehlich Apr 28, 2025

christophfroehlich Apr 28, 2025

christophfroehlich Apr 28, 2025

christophfroehlich Apr 28, 2025

christophfroehlich Apr 28, 2025

christophfroehlich Apr 28, 2025

		@@ -0,0 +1,215 @@
		// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)

	description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.",
	description: "Choose whether a TwistStamped or a AckermannDriveStamped message is used as input.",

Add AckermannDriveStamped control to steering library #1563

Are you sure you want to change the base?

Add AckermannDriveStamped control to steering library #1563

Conversation

wittenator commented Mar 1, 2025 • edited Loading

wittenator commented Mar 1, 2025 • edited Loading

christophfroehlich left a comment

Choose a reason for hiding this comment

wittenator commented Apr 11, 2025

wittenator commented Apr 24, 2025

christophfroehlich commented Apr 24, 2025

wittenator commented Apr 24, 2025

wittenator commented Apr 26, 2025 • edited Loading

codecov bot commented Apr 26, 2025 • edited Loading

Codecov Report

mergify bot commented Apr 27, 2025

Juliaj left a comment

Choose a reason for hiding this comment

Juliaj Apr 28, 2025 • edited Loading

Choose a reason for hiding this comment

Juliaj Apr 28, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

christophfroehlich left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

wittenator commented Mar 1, 2025 •

edited

Loading

wittenator commented Mar 1, 2025 •

edited

Loading

wittenator commented Apr 26, 2025 •

edited

Loading

codecov bot commented Apr 26, 2025 •

edited

Loading

Juliaj Apr 28, 2025 •

edited

Loading

Juliaj Apr 28, 2025 •

edited

Loading