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

(cherry picked from commit 1004054) # Conflicts: # doc/release_notes.rst # steering_controllers_library/src/steering_controllers_library.cpp
ros-controls · Dec 29, 2024 · 053e8a9 · 053e8a9
1 parent 3cef571
commit 053e8a9
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 14 deletions.
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
@@ -53,7 +53,12 @@ pid_controller
 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>`_).
+<<<<<<< HEAD
 * 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>`_).
+=======
+* A fix for Ackermann steering odometry was added (`#921 <https://github.com/ros-controls/ros2_controllers/pull/921>`_).
+* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 <https://github.com/ros-controls/ros2_controllers/pull/1289>`_).
+>>>>>>> 1004054 (fix(timeout): do not reset steer wheels to 0. on timeout (#1289))
 
 gpio_controllers
 ************************

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -404,8 +404,18 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate(
 
 controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers()
 {
+<<<<<<< HEAD
   // Move functionality to the `update_and_write_commands` because of the missing arguments in
   // humble - otherwise issues with multiple time-sources might happen when working with simulators
+=======
+  auto current_ref = *(input_ref_.readFromRT());
+
+  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;
+  }
+>>>>>>> 1004054 (fix(timeout): do not reset steer wheels to 0. on timeout (#1289))
 
   return controller_interface::return_type::OK;
 }
@@ -452,14 +462,23 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
     last_linear_velocity_ = reference_interfaces_[0];
     last_angular_velocity_ = reference_interfaces_[1];
 
+<<<<<<< HEAD
     auto [traction_commands, steering_commands] = odometry_.get_commands(
       last_linear_velocity_, last_angular_velocity_, params_.open_loop,
       params_.reduce_wheel_speed_until_steering_reached);
+=======
+    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);
+>>>>>>> 1004054 (fix(timeout): do not reset steer wheels to 0. on timeout (#1289))
     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 +490,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)