Extend odometry tests for steering controllers #1606

AdityaPawar162 · 2025-03-24T18:40:57Z

This PR addresses issue #1456 by:
1.Tracking odometry values before and after the timeout.
2.Ensuring position values remain stable.
3.Verifying that linear and angular velocities reset to zero.

codecov · 2025-03-24T19:04:15Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 85.15%. Comparing base (eca84fa) to head (590f747).
Report is 1 commits behind head on master.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1606      +/-   ##
==========================================
+ Coverage   85.10%   85.15%   +0.05%     
==========================================
  Files         123      123              
  Lines       11699    11712      +13     
  Branches      995      995              
==========================================
+ Hits         9956     9973      +17     
+ Misses       1432     1428       -4     
  Partials      311      311

Flag	Coverage Δ
unittests	`85.15% <100.00%> (+0.05%)`	⬆️

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

Files with missing lines	Coverage Δ
...library/test/test_steering_controllers_library.cpp	`100.00% <100.00%> (ø)`

... and 5 files with indirect coverage changes

🚀 New features to boost your workflow:

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

christophfroehlich

Thanks for giving a try! But apparently, this does not test the original issue. What I tried: Run colcon build and test with this version, and then with
git revert 2375aca
It succeeds in both cases.

steering_controllers_library/test/test_steering_controllers_library.cpp

AdityaPawar162 · 2025-03-25T09:18:30Z

Hi @christophfroehlich,
I have updated the function as per suggested changes,I have also added a check for the last_linear_velocity_ and last_angular_velocity_.

christophfroehlich · 2025-03-25T10:00:35Z

tests are failing now

AdityaPawar162 · 2025-03-25T10:02:45Z

tests are failing now

Sorry , for the incorrect push.

AdityaPawar162 · 2025-03-25T11:12:10Z

Hi @christophfroehlich ,
I have pushed the correct changes, please do suggest any changes to be made.

christophfroehlich

Thanks, the tests for the last*velocity now fails without the bugfix.

I had a look now why the other test does not fail:
1.) The test is in open_loop=false mode
2.) The update_odometry call in update_and_write_commands is overriden in the TestableSteeringControllersLibrary and does not update the odometry class.

We should add a new testfile with a different yaml (see add_rostest_with_parameters_gmock in the CMakeLists file) and add something like this in the TestableSteeringControllersLibrary::update_odometry (from bicycle_steering_controller)

  if (params_.open_loop)
  {
    odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
  }

AdityaPawar162 · 2025-03-25T13:41:11Z

Thanks, the tests for the last*velocity now fails without the bugfix.

I had a look now why the other test does not fail: 1.) The test is in open_loop=false mode 2.) The update_odometry call in update_and_write_commands is overriden in the TestableSteeringControllersLibrary and does not update the odometry class.

We should add a new testfile with a different yaml (see add_rostest_with_parameters_gmock in the CMakeLists file) and add something like this in the TestableSteeringControllersLibrary::update_odometry (from bicycle_steering_controller)
  if (params_.open_loop)
  {
    odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
  }

ok

AdityaPawar162 · 2025-03-26T18:23:32Z

Hi @christophfroehlich,
As per your suggestion, I created a new YAML file with open_loop set to true and added a separate test file named test_steering_controllers_open_library. I also modified the TestableSteeringControllersLibrary::update_geometry function. However, I'm unsure why the builds are failing.

steering_controllers_library/CMakeLists.txt

steering_controllers_library/test/steering_controllers_library_open_loop_timeout_params.yaml

christophfroehlich

See my other comments. With these changes, the test runs but fails with

3: /workspaces/ros2_rolling_ws/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:173: Failure
3: The difference between controller_->command_interfaces_[2].get_value() and 0.575875 is 0.071721078654024661, which exceeds 1e-6, where
3: controller_->command_interfaces_[2].get_value() evaluates to 0.50415392134597536,
3: 0.575875 evaluates to 0.57587500000000003, and
3: 1e-6 evaluates to 9.9999999999999995e-07.
3: 
3: /workspaces/ros2_rolling_ws/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:174: Failure
3: The difference between controller_->command_interfaces_[3].get_value() and 0.575875 is 0.092000372941643649, which exceeds 1e-6, where
3: controller_->command_interfaces_[3].get_value() evaluates to 0.66787537294164367,
3: 0.575875 evaluates to 0.57587500000000003, and
3: 1e-6 evaluates to 9.9999999999999995e-07.
3: 
3: /workspaces/ros2_rolling_ws/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:239: Failure
3: The difference between controller_->command_interfaces_[2].get_value() and 0.575875 is 0.071721078654024661, which exceeds 1e-6, where
3: controller_->command_interfaces_[2].get_value() evaluates to 0.50415392134597536,
3: 0.575875 evaluates to 0.57587500000000003, and
3: 1e-6 evaluates to 9.9999999999999995e-07.
3: 
3: /workspaces/ros2_rolling_ws/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:240: Failure
3: The difference between controller_->command_interfaces_[3].get_value() and 0.575875 is 0.092000372941643649, which exceeds 1e-6, where
3: controller_->command_interfaces_[3].get_value() evaluates to 0.66787537294164367,
3: 0.575875 evaluates to 0.57587500000000003, and
3: 1e-6 evaluates to 9.9999999999999995e-07.

I'm not sure why these are different in open_loop mode, but haven't investigated further now

steering_controllers_library/test/test_steering_controllers_library.cpp

AdityaPawar162 · 2025-03-27T14:43:56Z

Hi @christophfroehlich,
I have made suggested changes, please do take a look.

christophfroehlich

Tests are failing now with the output I posted above. Please have a look why this differs in open-loop mode

steering_controllers_library/src/steering_controllers_library.cpp

christophfroehlich · 2025-03-29T19:24:54Z

While checking the test class, I see that there are some unused/duplicate constants.

Could you clean them up as well please?

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Lines 57 to 61 in d1844e6

    
           static constexpr double WHEELBASE_ = 3.24644; 
        
           static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; 
        
           static constexpr double REAR_WHEEL_TRACK_ = 1.76868; 
        
           static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; 
        
           static constexpr double REAR_WHEELS_RADIUS_ = 0.45;

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Lines 292 to 295 in d1844e6

    
           double reference_timeout_ = 2.0; 
        
           bool front_steering_ = true; 
        
           bool open_loop_ = false; 
        
           unsigned int velocity_rolling_window_size_ = 10;

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Lines 300 to 309 in d1844e6

    
           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_ = { 
        
             "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; 
        
           std::vector<std::string> front_wheels_preceeding_names_ = { 
        
             "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; 
        
           std::vector<std::string> preceeding_joint_names_ = { 
        
             rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], 
        
             front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]};

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Lines 311 to 315 in d1844e6

    
           double wheelbase_ = 3.24644; 
        
           double front_wheel_track_ = 2.12321; 
        
           double rear_wheel_track_ = 1.76868; 
        
           double front_wheels_radius_ = 0.45; 
        
           double rear_wheels_radius_ = 0.45;

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Line 324 in d1844e6

std::string preceeding_prefix_ = "pid_controller";

christophfroehlich · 2025-03-29T19:37:31Z

steering_controllers_library/src/steering_controllers_library.cpp

-
-    if (params_.front_steering)
+    // calculate new commands if not in timeout
+    if (!timeout)


I don't see the necessity of this change.
In case of timeout it SHOULD set the appropriate commands see L397, L401 or L409/413, respectively.

Actually, I specifically wanted to update traction_commands and steering_commands when the timeout is false, so I thought there is a need to add this
auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); inside the flag.

AdityaPawar162 · 2025-03-29T19:55:03Z

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Lines 57 to 61 in d1844e6

    
           static constexpr double WHEELBASE_ = 3.24644; 
        
           static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; 
        
           static constexpr double REAR_WHEEL_TRACK_ = 1.76868; 
        
           static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; 
        
           static constexpr double REAR_WHEELS_RADIUS_ = 0.45;

This is getting used in the function configure_odometry

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Lines 121 to 128 in d1844e6

    
           controller_interface::CallbackReturn configure_odometry() 
        
           { 
        
             set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); 
        
             odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); 
        
             odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); 
        
             return controller_interface::CallbackReturn::SUCCESS; 
        
           }

christophfroehlich · 2025-03-29T19:57:57Z

This is getting used in the function configure_odometry

only FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_

AdityaPawar162 · 2025-03-30T18:10:03Z

Hi @christophfroehlich,
The joint_command_values are hardcoded to 2.2 and 4.4, I think so because of this the steering angle is not getting updated ?

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Line 318 in d1844e6

std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};

Extend odometry tests for steering controllers

4f07022

github-actions bot requested review from bmagyar, christophfroehlich, homalozoa, mcbed, olivier-stasse, qiayuanl and rosterloh March 24, 2025 18:41

christophfroehlich requested changes Mar 24, 2025

View reviewed changes

steering_controllers_library/test/test_steering_controllers_library.cpp Outdated Show resolved Hide resolved

steering_controllers_library/test/test_steering_controllers_library.cpp Show resolved Hide resolved

Updated the post_odom sequence and added last velocities check

618c36d

Fixed pointer

590f747

christophfroehlich requested changes Mar 25, 2025

View reviewed changes

Added a new parameters yaml and updated CMakelists.txt

551a67e

christophfroehlich reviewed Mar 26, 2025

View reviewed changes

steering_controllers_library/CMakeLists.txt Outdated Show resolved Hide resolved

christophfroehlich reviewed Mar 26, 2025

View reviewed changes

steering_controllers_library/test/steering_controllers_library_open_loop_timeout_params.yaml Outdated Show resolved Hide resolved

christophfroehlich requested changes Mar 26, 2025

View reviewed changes

christophfroehlich reviewed Mar 26, 2025

View reviewed changes

steering_controllers_library/test/test_steering_controllers_library.cpp Show resolved Hide resolved

christophfroehlich reviewed Mar 26, 2025

View reviewed changes

steering_controllers_library/test/test_steering_controllers_library.cpp Show resolved Hide resolved

Removed redundant gmock target and applied requested modifications

d059bcc

Merge branch 'master' into fix-extend-odometry-tests

66ac359

christophfroehlich requested changes Mar 28, 2025

View reviewed changes

Merge branch 'master' into fix-extend-odometry-tests

9e9824b

Modified steering_controller_library to handle timeout

9b5e976

christophfroehlich reviewed Mar 29, 2025

View reviewed changes

steering_controllers_library/src/steering_controllers_library.cpp Show resolved Hide resolved

christophfroehlich reviewed Mar 29, 2025

View reviewed changes

Removed duplicate constants

891d4ec

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Extend odometry tests for steering controllers #1606

Extend odometry tests for steering controllers #1606

AdityaPawar162 commented Mar 24, 2025

codecov bot commented Mar 24, 2025 •

edited

Loading

christophfroehlich left a comment

AdityaPawar162 commented Mar 25, 2025

christophfroehlich commented Mar 25, 2025

AdityaPawar162 commented Mar 25, 2025

AdityaPawar162 commented Mar 25, 2025

christophfroehlich left a comment •

edited

Loading

AdityaPawar162 commented Mar 25, 2025

AdityaPawar162 commented Mar 26, 2025

christophfroehlich left a comment

AdityaPawar162 commented Mar 27, 2025

christophfroehlich left a comment

christophfroehlich commented Mar 29, 2025

christophfroehlich Mar 29, 2025

AdityaPawar162 Mar 29, 2025 •

edited

Loading

AdityaPawar162 commented Mar 29, 2025

christophfroehlich commented Mar 29, 2025

AdityaPawar162 commented Mar 30, 2025

Extend odometry tests for steering controllers #1606

Are you sure you want to change the base?

Extend odometry tests for steering controllers #1606

Conversation

AdityaPawar162 commented Mar 24, 2025

codecov bot commented Mar 24, 2025 • edited Loading

Codecov Report

christophfroehlich left a comment

Choose a reason for hiding this comment

AdityaPawar162 commented Mar 25, 2025

christophfroehlich commented Mar 25, 2025

AdityaPawar162 commented Mar 25, 2025

AdityaPawar162 commented Mar 25, 2025

christophfroehlich left a comment • edited Loading

Choose a reason for hiding this comment

AdityaPawar162 commented Mar 25, 2025

AdityaPawar162 commented Mar 26, 2025

christophfroehlich left a comment

Choose a reason for hiding this comment

AdityaPawar162 commented Mar 27, 2025

christophfroehlich left a comment

Choose a reason for hiding this comment

christophfroehlich commented Mar 29, 2025

christophfroehlich Mar 29, 2025

Choose a reason for hiding this comment

AdityaPawar162 Mar 29, 2025 • edited Loading

Choose a reason for hiding this comment

AdityaPawar162 commented Mar 29, 2025

christophfroehlich commented Mar 29, 2025

AdityaPawar162 commented Mar 30, 2025

codecov bot commented Mar 24, 2025 •

edited

Loading

christophfroehlich left a comment •

edited

Loading

AdityaPawar162 Mar 29, 2025 •

edited

Loading