fix(timeout): do not reset steer wheels to 0. on timeout (backport #1289

) (#1452)
ros-controls · Dec 29, 2024 · e247c1c · e247c1c
1 parent 3cef571
commit e247c1c
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 33 deletions.
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
@@ -54,6 +54,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>`_).
 * 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>`_).
+* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 <https://github.com/ros-controls/ros2_controllers/pull/1289>`_).
 
 gpio_controllers
 ************************

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -416,26 +416,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
   if (!is_in_chained_mode())
   {
     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<double>::quiet_NaN();
-        current_ref->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
-      }
+      reference_interfaces_[0] = current_ref->twist.linear.x;
+      reference_interfaces_[1] = current_ref->twist.angular.z;
     }
   }
 
@@ -452,14 +436,18 @@ 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,
       params_.reduce_wheel_speed_until_steering_reached);
     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++)
       {
@@ -471,7 +459,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++)
         {

diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp
@@ -136,19 +136,22 @@ 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_.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]));
   for (const auto & interface : controller_->reference_interfaces_)
   {
     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;
@@ -177,19 +180,22 @@ 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_.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]));
   for (const auto & interface : controller_->reference_interfaces_)
   {
     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)