reset odometry service update - tricycle controller

tricycle_controller/include/tricycle_controller/tricycle_controller.hpp

@@ -19,6 +19,7 @@
 #ifndef TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_
 #define TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_
 
+#include <atomic>
 #include <chrono>
 #include <cmath>
 #include <memory>
@@ -76,6 +77,11 @@ class TricycleController : public controller_interface::ControllerInterface
 
   CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
 
+  void reset_odometry(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+    std::shared_ptr<std_srvs::srv::Empty::Response> res);
+
 protected:
   struct TractionHandle
   {
@@ -132,17 +138,14 @@ class TricycleController : public controller_interface::ControllerInterface
   std::shared_ptr<TwistStamped> last_command_msg_;
 
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_odom_service_;
+  std::atomic<bool> reset_odom_{false};
 
   std::queue<AckermannDrive> previous_commands_;  // last two commands
 
   // speed limiters
   TractionLimiter limiter_traction_;
   SteeringLimiter limiter_steering_;
 
-  void reset_odometry(
-    const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<std_srvs::srv::Empty::Request> req,
-    std::shared_ptr<std_srvs::srv::Empty::Response> res);
   bool reset();
   void halt();
 };

tricycle_controller/src/tricycle_controller.cpp

@@ -138,6 +138,13 @@ controller_interface::return_type TricycleController::update(
     odometry_.update(Ws_read, alpha_read, period);
   }
 
+  // check if odom reset was requested by non-RT thread
+  if (reset_odom_.load())
+  {
+    odometry_.resetOdometry();
+    reset_odom_.store(false);
+  }
+
   tf2::Quaternion orientation;
   orientation.setRPY(0.0, 0.0, odometry_.getHeading());
 
@@ -425,8 +432,9 @@ void TricycleController::reset_odometry(
   const std::shared_ptr<std_srvs::srv::Empty::Request> /*req*/,
   std::shared_ptr<std_srvs::srv::Empty::Response> /*res*/)
 {
-  odometry_.resetOdometry();
-  RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset");
+  // set the reset flag for thread-safe odometry reset
+  reset_odom_.store(true);
+  RCLCPP_INFO(get_node()->get_logger(), "Odometry reset requested");
 }
 
 bool TricycleController::reset()

tricycle_controller/test/test_tricycle_controller.cpp

@@ -50,6 +50,7 @@ const char steering_joint_name[] = "steering_joint";
 class TestableTricycleController : public tricycle_controller::TricycleController
 {
 public:
+  using TricycleController::odometry_;
   using TricycleController::TricycleController;
   std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
   {
@@ -372,3 +373,72 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters)
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
   executor.cancel();
 }
+
+TEST_F(TestTricycleController, odometry_reset_service)
+{
+  // 0. correctly initialize and activate the controller
+  ASSERT_EQ(
+    InitController(
+      traction_joint_name, steering_joint_name,
+      {rclcpp::Parameter("wheelbase", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}),
+    controller_interface::return_type::OK);
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  auto state = controller_->configure();
+  assignResources();
+
+  ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
+  EXPECT_EQ(position_, steering_joint_pos_cmd_->get_optional().value());
+  EXPECT_EQ(velocity_, traction_joint_vel_cmd_->get_optional().value());
+
+  state = controller_->get_node()->activate();
+  ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
+
+  rclcpp::Time test_time(0, 0, RCL_ROS_TIME);
+  rclcpp::Duration period = rclcpp::Duration::from_seconds(0.1);
+
+  // 1. move the robot first
+  const double linear = 1.0;
+  const double angular = 0.0;
+  publish(linear, angular);
+  controller_->wait_for_twist(executor);
+  controller_->update(test_time, period);
+  test_time += period;
+
+  // verify it actually moved
+  ASSERT_GT(controller_->odometry_.getX(), 0.0);
+
+  // 2. stop the robot and trigger the reset callback
+  publish(0.0, 0.0);
+  controller_->wait_for_twist(executor);
+  auto request = std::make_shared<std_srvs::srv::Empty::Request>();
+  auto response = std::make_shared<std_srvs::srv::Empty::Response>();
+  controller_->reset_odometry(nullptr, request, response);
+
+  // run update to process the reset request
+  // then, verify the odometry values are now back to 0.0
+  controller_->update(test_time, period);
+  test_time += period;
+  EXPECT_EQ(controller_->odometry_.getX(), 0.0);
+  EXPECT_EQ(controller_->odometry_.getY(), 0.0);
+
+  // 3. move the robot again
+  publish(linear, angular);
+  controller_->wait_for_twist(executor);
+
+  // run update twice to ensure velocity integrated and the reset flag was cleared
+  controller_->update(test_time, period);
+  test_time += period;
+  controller_->update(test_time, period);
+  ASSERT_GT(controller_->odometry_.getX(), 0.0);
+
+  // 4. deactivate and cleanup
+  state = controller_->get_node()->deactivate();
+  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+  state = controller_->get_node()->cleanup();
+  ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
+  state = controller_->configure();
+  ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
+  executor.cancel();
+}