diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 81a62b989c..b8aa29e92b 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/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 #include #include #include @@ -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 request_header, + const std::shared_ptr req, + std::shared_ptr res); + protected: struct TractionHandle { @@ -132,6 +138,7 @@ class TricycleController : public controller_interface::ControllerInterface std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_; + std::atomic reset_odom_{false}; std::queue previous_commands_; // last two commands @@ -139,10 +146,6 @@ class TricycleController : public controller_interface::ControllerInterface TractionLimiter limiter_traction_; SteeringLimiter limiter_steering_; - void reset_odometry( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr res); bool reset(); void halt(); }; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index aef62e7b0f..985e654e97 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/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 /*req*/, std::shared_ptr /*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() diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 545283d5f2..f346b3c475 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/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 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(); + auto response = std::make_shared(); + 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(); +}