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

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

Extend odometry tests for steering controllers #1606

Changes from 3 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
46 changes: 46 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,13 +123,33 @@ 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);

// 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]));
for (const auto & interface : controller_->reference_interfaces_)
Expand All @@ -153,9 +173,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 @@ -174,6 +203,19 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
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]));
for (const auto & interface : controller_->reference_interfaces_)
Expand All @@ -196,6 +238,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