From d70681258a7a83789b2c3eb73e9613776a6aa933 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Fri, 13 Sep 2024 10:45:44 +0200 Subject: [PATCH 1/5] fix(timeout): do not reset steer wheel to 0. on timeout Instead of setting the incoming twist command velocity to zero, we should only set the wheel commands to zero without altering the steering angles in order to avoid in-place steering of the wheels. --- .../src/steering_controllers_library.cpp | 29 ++++++------------- 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f193098f82..836574f150 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -355,26 +355,11 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; - } - } - else - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; } return controller_interface::return_type::OK; @@ -396,13 +381,17 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; + const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; + const auto timeout = + age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + auto [traction_commands, steering_commands] = odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { @@ -414,7 +403,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { From 86015b2f86c2f9c096b3a62e230067869f74a2e7 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Thu, 26 Sep 2024 15:57:50 +0200 Subject: [PATCH 2/5] fix unit tests Signed-off-by: Rein Appeldoorn --- .../test_steering_controllers_library.cpp | 30 +++++++++++-------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index fca7d00946..8b21ea6bf6 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -133,8 +133,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -142,10 +142,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // 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); // case 2 position_feedback = true controller_->params_.position_feedback = true; @@ -174,8 +177,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -183,10 +186,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // 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); } int main(int argc, char ** argv) From ca0cefc5028707c5ca55408178aa18c52b2e263e Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Thu, 26 Sep 2024 16:00:34 +0200 Subject: [PATCH 3/5] release notes Signed-off-by: Rein Appeldoorn --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b55db861b3..1fb66c4475 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -56,6 +56,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 `_). * A fix for Ackermann steering odometry was added (`#921 `_). +* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). tricycle_controller ************************ From 0ed588087f3fc7e52fbc6050a5c118154da0315e Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Fri, 27 Sep 2024 11:14:56 +0200 Subject: [PATCH 4/5] Update steering_controllers_library/test/test_steering_controllers_library.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../test/test_steering_controllers_library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 8b21ea6bf6..257bf9e311 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -133,7 +133,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); From c8794cf222635cba9a648d2ecf6feea68c9a0694 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Fri, 27 Sep 2024 11:15:01 +0200 Subject: [PATCH 5/5] Update steering_controllers_library/test/test_steering_controllers_library.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../test/test_steering_controllers_library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 257bf9e311..e73f32cd5e 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -177,7 +177,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));