Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

steering_controllers_library: Add reduce_wheel_speed_until_steering_reached parameter #1314

Merged
merged 5 commits into from
Dec 17, 2024

Conversation

christophfroehlich
Copy link
Contributor

@christophfroehlich christophfroehlich commented Oct 13, 2024

Some features are missing in the library to feature-equally integrate the older tricycle_controller (#850).

reduce_wheel_speed_until_steering_reached is one step closer.

The old code

// Reduce wheel speed until the target angle has been reached
double alpha_delta = abs(alpha_write - alpha_read);
double scale;
if (alpha_delta < M_PI / 6)
{
scale = 1;
}
else if (alpha_delta > M_PI_2)
{
scale = 0.01;
}
else
{
// TODO(anyone): find the best function, e.g convex power functions
scale = cos(alpha_delta);
}
Ws_write *= scale;

I modified the function a bit to be continuously differentiable
image

This breaks ABI of the odometry class, but I don't think that lots of people use this directly without the derived controllers.

@christophfroehlich christophfroehlich added backport-humble This label should be used by maintainers only! Label triggers PR backport to ROS2 humble. backport-iron labels Oct 13, 2024
Copy link

codecov bot commented Oct 13, 2024

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 83.82%. Comparing base (d32665a) to head (954c9f2).
Report is 3 commits behind head on master.

Additional details and impacted files
@@            Coverage Diff             @@
##           master    #1314      +/-   ##
==========================================
+ Coverage   83.71%   83.82%   +0.10%     
==========================================
  Files         122      122              
  Lines       11050    11120      +70     
  Branches      940      946       +6     
==========================================
+ Hits         9251     9321      +70     
  Misses       1489     1489              
  Partials      310      310              
Flag Coverage Δ
unittests 83.82% <100.00%> (+0.10%) ⬆️

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

Files with missing lines Coverage Δ
...steering_controllers_library/steering_odometry.hpp 100.00% <ø> (ø)
...llers_library/src/steering_controllers_library.cpp 79.72% <100.00%> (+0.09%) ⬆️
...ring_controllers_library/src/steering_odometry.cpp 82.53% <100.00%> (+1.00%) ⬆️
...ontrollers_library/test/test_steering_odometry.cpp 100.00% <100.00%> (ø)

Copy link
Member

@destogl destogl left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A small comment about docs, otherwise great!

I agree to break this on Humble

@@ -57,6 +57,7 @@ steering_controllers_library
* Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 <https://github.com/ros-controls/ros2_controllers/pull/927>`_).
* A fix for Ackermann steering odometry was added (`#921 <https://github.com/ros-controls/ros2_controllers/pull/921>`_).
* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 <https://github.com/ros-controls/ros2_controllers/pull/1289>`_).
* New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 <https://github.com/ros-controls/ros2_controllers/pull/1314>`_).
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need this "new", if yes for how long we keep it that way?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is part of the release notes, so I think that "new" is appropriate here?

Copy link
Member

@bmagyar bmagyar left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you!

@bmagyar bmagyar merged commit 0438a1e into master Dec 17, 2024
21 of 26 checks passed
@bmagyar bmagyar deleted the feature/steering_controllers_library_wait_steering branch December 17, 2024 06:44
mergify bot pushed a commit that referenced this pull request Dec 17, 2024
…reached` parameter (#1314)

(cherry picked from commit 0438a1e)

# Conflicts:
#	doc/release_notes.rst
#	steering_controllers_library/src/steering_controllers_library.cpp
christophfroehlich added a commit that referenced this pull request Dec 17, 2024
christophfroehlich added a commit that referenced this pull request Dec 17, 2024
…reached` parameter (backport #1314) (#1429)

Co-authored-by: Christoph Fröhlich <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
backport-humble This label should be used by maintainers only! Label triggers PR backport to ROS2 humble.
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants