reset odom service removed

- changes in diff drive PR (#2096) mirrored
- manual state transition added for testing
- docs updated

Author: kuralme

doc/release_notes.rst

@@ -21,8 +21,9 @@ diff_drive_controller
 
 mecanum_drive_controller
 *****************************
-* Parameter ``tf_frame_prefix_enable`` got deprecated and will be removed in a future release (`#1997 <https://github.com/ros-controls/ros2_controllers/pull/1997>`_).
-* Now any tilde ("~") character in ``tf_frame_prefix`` is substituted with node namespace. (`#1997 <https://github.com/ros-controls/ros2_controllers/pull/1997>`_).
+* Parameter ``tf_frame_prefix_enable`` got deprecated and will be removed in a future release (`#2063 <https://github.com/ros-controls/ros2_controllers/pull/2063>`_).
+* Now any tilde ("~") character in ``tf_frame_prefix`` is substituted with node namespace. (`#2063 <https://github.com/ros-controls/ros2_controllers/pull/2063>`_).
+* Set odometry service added to be used at runtime. (`#2110 <https://github.com/ros-controls/ros2_controllers/pull/2110>`_).
 
 joint_state_broadcaster
 ************************

mecanum_drive_controller/CMakeLists.txt

@@ -54,7 +54,7 @@ target_link_libraries(mecanum_drive_controller PUBLIC
                       ${tf2_geometry_msgs_TARGETS}
                       ${tf2_msgs_TARGETS}
                       ${nav_msgs_TARGETS}
-                      ${std_srvs_TARGETS})
+                      ${control_msgs_TARGETS})
 
 pluginlib_export_plugin_description_file(
   controller_interface mecanum_drive_controller.xml)

mecanum_drive_controller/doc/userdoc.rst

@@ -60,6 +60,11 @@ Publishers
 - ``<controller_name>/tf_odometry``       [``tf2_msgs/msg/TFMessage``]
 - ``<controller_name>/controller_state``  [``control_msgs/msg/MecanumDriveControllerState``]
 
+Services
+,,,,,,,,,,,
+~/set_odometry [control_msgs::srv::SetOdometry]
+  This service can be used to set the current odometry of the robot to desired values.
+
 Parameters
 ,,,,,,,,,,,
 

mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp

@@ -33,8 +33,6 @@
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "realtime_tools/realtime_thread_safe_box.hpp"
-#include "std_srvs/srv/empty.hpp"
-#include "std_srvs/srv/set_bool.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
 #include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp"
@@ -80,10 +78,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<control_msgs::srv::SetOdometry::Request> req,
     std::shared_ptr<control_msgs::srv::SetOdometry::Response> res);
-  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);
 
   using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped;
   using OdomStateMsg = nav_msgs::msg::Odometry;
@@ -153,12 +147,9 @@ class MecanumDriveController : public controller_interface::ChainableControllerI
 
   Odometry odometry_;
   rclcpp::Service<control_msgs::srv::SetOdometry>::SharedPtr set_odom_service_;
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_odom_service_;
-  std::atomic<bool> set_odom_request_{false}, reset_odom_request_{false};
-  struct
-  {
-    double x, y, yaw;
-  } requested_odom_params_;
+  std::atomic<bool> set_odom_requested_{false};
+  realtime_tools::RealtimeThreadSafeBox<control_msgs::srv::SetOdometry::Request>
+    requested_odom_params_;
 
 private:
   // callback for topic interface

mecanum_drive_controller/package.xml

@@ -36,7 +36,6 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>realtime_tools</depend>
   <depend>rcpputils</depend>
-  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_msgs</depend>

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -46,7 +46,6 @@ void reset_controller_reference_msg(
 }
 
 constexpr auto DEFAULT_SET_ODOM_SERVICE = "~/set_odometry";
-constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry";
 }  // namespace
 
 namespace mecanum_drive_controller
@@ -250,17 +249,11 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
 
   try
   {
-    // Create odometry set & reset services
     set_odom_service_ = get_node()->create_service<control_msgs::srv::SetOdometry>(
       DEFAULT_SET_ODOM_SERVICE,
       std::bind(
         &MecanumDriveController::set_odometry, this, std::placeholders::_1, std::placeholders::_2,
         std::placeholders::_3));
-    reset_odom_service_ = get_node()->create_service<std_srvs::srv::Empty>(
-      DEFAULT_RESET_ODOM_SERVICE,
-      std::bind(
-        &MecanumDriveController::reset_odometry, this, std::placeholders::_1, std::placeholders::_2,
-        std::placeholders::_3));
   }
   catch (const std::exception & e)
   {
@@ -372,7 +365,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate(
   ControllerReferenceMsg emtpy_msg;
   reset_controller_reference_msg(emtpy_msg, get_node());
   input_ref_.try_set(emtpy_msg);
-
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -476,16 +468,15 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
   const double wheel_rear_left_state_vel = wheel_rear_left_state_vel_op.value();
 
   // check if odometry set or reset was requested by non-RT thread
-  if (set_odom_request_.load())
-  {
-    odometry_.setOdometry(
-      requested_odom_params_.x, requested_odom_params_.y, requested_odom_params_.yaw);
-    set_odom_request_.store(false);
-  }
-  else if (reset_odom_request_.load())
+  if (set_odom_requested_.load())
   {
-    odometry_.resetOdometry();
-    reset_odom_request_.store(false);
+    auto param_op = requested_odom_params_.try_get();
+    if (param_op.has_value())
+    {
+      auto params = param_op.value();
+      odometry_.setOdometry(params.x, params.y, params.yaw);
+      set_odom_requested_.store(false);
+    }
   }
   else
   {
@@ -630,23 +621,21 @@ void MecanumDriveController::set_odometry(
   const std::shared_ptr<control_msgs::srv::SetOdometry::Request> req,
   std::shared_ptr<control_msgs::srv::SetOdometry::Response> res)
 {
+  if (get_node()->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    res->success = false;
+    res->message = "Controller is not active";
+    return;
+  }
+
+  // put requested odom params into RealtimeThreadSafeBox
+  requested_odom_params_.set(*req);
+
   // flip the flag for thread-safe odom set in the control loop
-  set_odom_request_.store(true);
-  requested_odom_params_.x = req->x;
-  requested_odom_params_.y = req->y;
-  requested_odom_params_.yaw = req->yaw;
-  res->success = true;
-  res->message = "Odometry set requested";
-}
+  set_odom_requested_.store(true);
 
-void MecanumDriveController::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*/)
-{
-  // flip the flag for thread-safe odom reset in the control loop
-  reset_odom_request_.store(true);
-  RCLCPP_INFO(get_node()->get_logger(), "Odometry reset requested");
+  res->success = true;
+  res->message = "Odometry set request accepted";
 }
 
 }  // namespace mecanum_drive_controller

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

@@ -810,9 +810,16 @@ TEST_F(MecanumDriveControllerTest, odometry_set_reset_services)
   // 0. Initialize and activate
   SetUpController();
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->get_node()->trigger_transition(
+    rclcpp_lifecycle::Transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE));
 
   controller_->set_chained_mode(true);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->get_node()->trigger_transition(
+    rclcpp_lifecycle::Transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE));
+  ASSERT_EQ(
+    controller_->get_node()->get_current_state().id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
 
   const double dt = 0.02;  // 50Hz
   rclcpp::Time test_time = controller_->get_node()->now();
@@ -843,23 +850,7 @@ TEST_F(MecanumDriveControllerTest, odometry_set_reset_services)
   for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0);
   ASSERT_GT(controller_->odometry_.getX(), 0.0);
 
-  // 2. Call Odometry Reset Service
-  auto reset_request = std::make_shared<std_srvs::srv::Empty::Request>();
-  auto reset_response = std::make_shared<std_srvs::srv::Empty::Response>();
-  controller_->reset_odometry(nullptr, reset_request, reset_response);
-
-  // One update to propagate the internal reset
-  controller_->update(test_time, period);
-
-  EXPECT_NEAR(controller_->odometry_.getX(), 0.0, 1e-6);
-  EXPECT_NEAR(controller_->odometry_.getY(), 0.0, 1e-6);
-  EXPECT_NEAR(controller_->odometry_.getRz(), 0.0, 1e-6);
-
-  // 3. Move again to ensure it still works
-  for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0);
-  ASSERT_GT(controller_->odometry_.getX(), 0.0);
-
-  // 4. Call Set Odometry Service
+  // 2. Call Set Odometry Service
   auto set_request = std::make_shared<control_msgs::srv::SetOdometry::Request>();
   auto set_response = std::make_shared<control_msgs::srv::SetOdometry::Response>();
   set_request->x = 5.0;
@@ -875,7 +866,7 @@ TEST_F(MecanumDriveControllerTest, odometry_set_reset_services)
   EXPECT_NEAR(controller_->odometry_.getY(), -2.0, 1e-6);
   EXPECT_NEAR(controller_->odometry_.getRz(), 1.57079632679, 1e-5);
 
-  // 5. Move forward again to verify
+  // 3. Move forward again to verify
   double start_y = controller_->odometry_.getY();
   for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0);  // we are facing +Y now
   EXPECT_GT(controller_->odometry_.getY(), start_y);

mecanum_drive_controller/test/test_mecanum_drive_controller.hpp

@@ -28,6 +28,8 @@
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "lifecycle_msgs/msg/transition.hpp"
 #include "mecanum_drive_controller/mecanum_drive_controller.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/executors.hpp"