Extend odometry tests for steering controllers

steering_controllers_library/CMakeLists.txt

@@ -74,6 +74,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/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

@@ -127,7 +127,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