Extend odometry tests for steering controllers

steering_controllers_library/CMakeLists.txt

@@ -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(

steering_controllers_library/src/steering_controllers_library.cpp

@@ -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);
+  }
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -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)
     {
-      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);
         }
       }
     }

steering_controllers_library/test/steering_controllers_library_open_loop_timeout_params.yaml

@@ -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

steering_controllers_library/test/test_steering_controllers_library.cpp

@@ -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]));
@@ -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);
@@ -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]));
@@ -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)

steering_controllers_library/test/test_steering_controllers_library.hpp

@@ -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
 {
@@ -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
@@ -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_;