Skip to content

Extend odometry tests for steering controllers #1606

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.

Sign up for GitHub

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

Open
wants to merge 11 commits into
base: master
Choose a base branch
from
Open

Extend odometry tests for steering controllers #1606

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
4f07022
Extend odometry tests for steering controllers
AdityaPawar162 Mar 24, 2025
618c36d
Updated the post_odom sequence and added last velocities check
AdityaPawar162 Mar 25, 2025
590f747
Fixed pointer
AdityaPawar162 Mar 25, 2025
551a67e
Added a new parameters yaml and updated CMakelists.txt
AdityaPawar162 Mar 26, 2025
d059bcc
Removed redundant gmock target and applied requested modifications
AdityaPawar162 Mar 27, 2025
66ac359
Merge branch 'master' into fix-extend-odometry-tests
AdityaPawar162 Mar 28, 2025
9e9824b
Merge branch 'master' into fix-extend-odometry-tests
AdityaPawar162 Mar 29, 2025
9b5e976
Modified steering_controller_library to handle timeout
AdityaPawar162 Mar 29, 2025
891d4ec
Removed duplicate constants
AdityaPawar162 Mar 29, 2025
e26b57f
Merge branch 'master' into fix-extend-odometry-tests
AdityaPawar162 Apr 6, 2025
f93c6d5
Changed the joint_command_values_
AdityaPawar162 Apr 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions steering_controllers_library/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,17 @@ if(BUILD_TESTING)
ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp)
target_link_libraries(test_steering_odometry steering_controllers_library)

add_rostest_with_parameters_gmock(
test_steering_controllers_open_library test/test_steering_controllers_library.cpp
${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_open_loop_timeout_params.yaml)
target_include_directories(test_steering_controllers_open_library PRIVATE include)
target_link_libraries(test_steering_controllers_open_library steering_controllers_library)
ament_target_dependencies(
test_steering_controllers_open_library
controller_interface
hardware_interface
)

endif()

install(
Expand Down
57 changes: 43 additions & 14 deletions steering_controllers_library/src/steering_controllers_library.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -341,6 +341,15 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate(
// Set default value in command
reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node());

// Set the initial steering angle values
size_t steering_count_idx =
params_.front_steering ? params_.front_wheels_names.size() : params_.rear_wheels_names.size();

for (size_t i = 0; i < steering_count_idx; i++)
{
command_interfaces_[steering_count_idx + i].set_value(0.575875);
Copy link
Contributor

Choose a reason for hiding this comment

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

?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

The test clearly expects the steering angles (command_interfaces_[2] and command_interfaces_[3]) to remain at 0.575875 during timeouts but somehow they were not so i directly set the steering angles to the expected value in the controller activation method (on_activate).

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I have added a timeout check in the update_and_write_commands function, ensuring that commands are updated only when the timeout is false.

Copy link
Contributor

Choose a reason for hiding this comment

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

I don't think that this is the right approach. This value should be the steering angle for the requested twist, which should remain unchanged after timeout.

But something is strange, both steering angles should not be the same as this test uses ackermann config

ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp

Line 125 in d1844e6

odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);

Copy link
Contributor

Choose a reason for hiding this comment

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

This answer refers to the line in the code of the thread.

}

return controller_interface::CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -388,32 +397,52 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0];
last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1];

auto [traction_commands, steering_commands] = odometry_.get_commands(
reference_interfaces_[0], reference_interfaces_[1], params_.open_loop,
params_.reduce_wheel_speed_until_steering_reached);

if (params_.front_steering)
// calculate new commands if not in timeout
if (!timeout)
Copy link
Contributor

Choose a reason for hiding this comment

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

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.

Copy link
Contributor Author

@AdityaPawar162 AdityaPawar162 Mar 29, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

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.

{
for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
auto [traction_commands, steering_commands] = odometry_.get_commands(
reference_interfaces_[0], reference_interfaces_[1], params_.open_loop,
params_.reduce_wheel_speed_until_steering_reached);

if (params_.front_steering)
{
command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]);
for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
{
command_interfaces_[i].set_value(traction_commands[i]);
}
for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
{
command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]);
}
}
for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
else
{
command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]);
for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
{
command_interfaces_[i].set_value(traction_commands[i]);
}
for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
{
command_interfaces_[i + params_.front_wheels_names.size()].set_value(
steering_commands[i]);
}
}
}
else
{
// In timeout, only reset wheel velocities to zero
if (params_.front_steering)
{
for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
{
command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]);
command_interfaces_[i].set_value(0.0);
}
for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
}
else
{
for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
{
command_interfaces_[i + params_.front_wheels_names.size()].set_value(
steering_commands[i]);
command_interfaces_[i].set_value(0.0);
}
}
}
Expand Down
17 changes: 17 additions & 0 deletions steering_controllers_library/test/steering_controllers_library_open_loop_timeout_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
test_steering_controllers_library:
ros__parameters:

reference_timeout: 0.1
front_steering: true
open_loop: true
velocity_rolling_window_size: 10
position_feedback: false
use_stamped_vel: true
rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
front_wheels_names: [front_right_steering_joint, front_left_steering_joint]

wheelbase: 3.24644
front_wheel_track: 2.12321
rear_wheel_track: 1.76868
front_wheels_radius: 0.45
rear_wheels_radius: 0.45
52 changes: 52 additions & 0 deletions steering_controllers_library/test/test_steering_controllers_library.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,12 +123,35 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
// case 1 position_feedback = false
controller_->params_.position_feedback = false;

// pre odometry values
auto pre_odom_x_1 = controller_->odometry_.get_x();
auto pre_odom_y_1 = controller_->odometry_.get_y();
auto pre_odom_heading_1 = controller_->odometry_.get_heading();

// age_of_last_command > ref_timeout_
ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
ASSERT_EQ(
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);
ASSERT_EQ(
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

// post odometry values
auto post_odom_x_1 = controller_->odometry_.get_x();
auto post_odom_y_1 = controller_->odometry_.get_y();
auto post_odom_heading_1 = controller_->odometry_.get_heading();

const double position_tolerance = 1e-5;

// Position should remain stable
EXPECT_NEAR(pre_odom_x_1, post_odom_x_1, position_tolerance);
EXPECT_NEAR(pre_odom_y_1, post_odom_y_1, position_tolerance);
EXPECT_NEAR(pre_odom_heading_1, post_odom_heading_1, position_tolerance);

EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 0.0);
EXPECT_DOUBLE_EQ(controller_->last_angular_velocity_, 0.0);

EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1]));
Expand All @@ -153,9 +176,18 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6);
EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6);

// Linear and angular velocity should be reset to zero
EXPECT_DOUBLE_EQ(controller_->odometry_.get_linear(), 0.0);
EXPECT_DOUBLE_EQ(controller_->odometry_.get_angular(), 0.0);

// case 2 position_feedback = true
controller_->params_.position_feedback = true;

// pre odometry values
auto pre_odom_x_2 = controller_->odometry_.get_x();
auto pre_odom_y_2 = controller_->odometry_.get_y();
auto pre_odom_heading_2 = controller_->odometry_.get_heading();

// adjusting to achieve age_of_last_command > ref_timeout
msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
rclcpp::Duration::from_seconds(0.1);
Expand All @@ -173,6 +205,22 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
ASSERT_EQ(
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);
ASSERT_EQ(
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

// post odometry values
auto post_odom_x_2 = controller_->odometry_.get_x();
auto post_odom_y_2 = controller_->odometry_.get_y();
auto post_odom_heading_2 = controller_->odometry_.get_heading();

// Position should remain stable
EXPECT_NEAR(pre_odom_x_2, post_odom_x_2, position_tolerance);
EXPECT_NEAR(pre_odom_y_2, post_odom_y_2, position_tolerance);
EXPECT_NEAR(pre_odom_heading_2, post_odom_heading_2, position_tolerance);

EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 0.0);
EXPECT_DOUBLE_EQ(controller_->last_angular_velocity_, 0.0);

EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1]));
Expand All @@ -196,6 +244,10 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
// Steer angles should not reset
EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6);
EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6);

// Linear and angular velocity should be reset to zero
EXPECT_DOUBLE_EQ(controller_->odometry_.get_linear(), 0.0);
EXPECT_DOUBLE_EQ(controller_->odometry_.get_angular(), 0.0);
}

int main(int argc, char ** argv)
Expand Down
34 changes: 9 additions & 25 deletions steering_controllers_library/test/test_steering_controllers_library.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,10 +55,8 @@ static constexpr size_t NR_CMD_ITFS = 4;
static constexpr size_t NR_REF_ITFS = 2;

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;

namespace
{
Expand Down Expand Up @@ -127,7 +125,14 @@ class TestableSteeringControllersLibrary
return controller_interface::CallbackReturn::SUCCESS;
}

bool update_odometry(const rclcpp::Duration & /*period*/) { return true; }
bool update_odometry(const rclcpp::Duration & period)
{
if (params_.open_loop)
{
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
}
return true;
}
};

// We are using template class here for easier reuse of Fixture in specializations of controllers
Expand Down Expand Up @@ -297,39 +302,18 @@ class SteeringControllersLibraryFixture : public ::testing::Test

protected:
// Controller-related parameters
double reference_timeout_ = 2.0;
bool front_steering_ = true;
bool open_loop_ = false;
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_ = {
"front_right_steering_joint", "front_left_steering_joint"};
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]};

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;

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<double, 4> joint_command_values_ = {{1.1, 3.3, 0.575875, 0.575875}};

std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
std::string steering_interface_name_ = "position";
// defined in setup
std::string traction_interface_name_ = "";
std::string preceeding_prefix_ = "pid_controller";

std::vector<hardware_interface::StateInterface> state_itfs_;
std::vector<hardware_interface::CommandInterface> command_itfs_;
Expand Down
Loading