Add set_odometry service - omni wheel drive controller (ros-controls#2148)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

Author: kuralme

doc/release_notes.rst

@@ -35,6 +35,7 @@ omni_wheel_drive_controller
 *****************************
 * Parameter ``tf_frame_prefix_enable`` got deprecated and will be removed in a future release (`#2073 <https://github.com/ros-controls/ros2_controllers/pull/2073>`_).
 * Now any tilde ("~") character in ``tf_frame_prefix`` is substituted with node namespace. (`#2073 <https://github.com/ros-controls/ros2_controllers/pull/2073>`_).
+* Set odometry service added to be used at runtime. (`#2148 <https://github.com/ros-controls/ros2_controllers/pull/2148>`_).
 
 joint_trajectory_controller
 ***************************

omni_wheel_drive_controller/CMakeLists.txt

@@ -8,6 +8,7 @@ export_windows_symbols()
 # Find dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   ament_cmake
+  control_msgs
   controller_interface
   generate_parameter_library
   geometry_msgs
@@ -52,6 +53,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
   ${geometry_msgs_TARGETS}
   ${nav_msgs_TARGETS}
   ${tf2_msgs_TARGETS}
+  ${control_msgs_TARGETS}
 )
 pluginlib_export_plugin_description_file(controller_interface omni_wheel_drive_plugin.xml)
 

omni_wheel_drive_controller/doc/userdoc.rst

@@ -64,6 +64,10 @@ Publishers
 /tf [tf2_msgs::msg::TFMessage]
   tf tree. Published only if ``enable_odom_tf=true``
 
+Services
+,,,,,,,,,,,
+~/set_odometry [control_msgs::srv::SetOdometry]
+  This service can be used to set the current odometry of the robot to desired values.
 
 Parameters
 ,,,,,,,,,,

omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp

@@ -32,7 +32,8 @@ class Odometry
   bool updateOpenLoop(
     const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel,
     const rclcpp::Time & time);
-  void resetOdometry();
+  [[deprecated("Use setOdometry(0.0, 0.0, 0.0) instead")]] void resetOdometry();
+  void setOdometry(const double & x, const double & y, const double & heading);
 
   double getX() const { return x_; }
   double getY() const { return y_; }

omni_wheel_drive_controller/include/omni_wheel_drive_controller/omni_wheel_drive_controller.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "control_msgs/srv/set_odometry.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
@@ -66,6 +67,11 @@ class OmniWheelDriveController : public controller_interface::ChainableControlle
   controller_interface::CallbackReturn on_error(
     const rclcpp_lifecycle::State & previous_state) override;
 
+  void set_odometry(
+    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);
+
 protected:
   std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
 
@@ -112,6 +118,11 @@ class OmniWheelDriveController : public controller_interface::ChainableControlle
   bool reset();
   bool on_set_chained_mode(bool chained_mode) override;
 
+  rclcpp::Service<control_msgs::srv::SetOdometry>::SharedPtr set_odom_service_;
+  std::atomic<bool> set_odom_requested_{false};
+  realtime_tools::RealtimeThreadSafeBox<control_msgs::srv::SetOdometry::Request>
+    requested_odom_params_;
+
 private:
   void reset_buffers();
 };

omni_wheel_drive_controller/package.xml

@@ -19,6 +19,7 @@
 
   <build_depend>ros2_control_cmake</build_depend>
 
+  <depend>control_msgs</depend>
   <depend>controller_interface</depend>
   <depend>eigen</depend>
   <depend>generate_parameter_library</depend>

omni_wheel_drive_controller/src/odometry.cpp

@@ -128,6 +128,12 @@ void Odometry::resetOdometry()
   y_ = 0.0;
   heading_ = 0.0;
 }
+void Odometry::setOdometry(const double & x, const double & y, const double & heading)
+{
+  x_ = x;
+  y_ = y;
+  heading_ = heading;
+}
 
 void Odometry::setParams(
   const double & robot_radius, const double & wheel_radius, const double & wheel_offset,

omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp

@@ -33,6 +33,7 @@ namespace
 constexpr auto DEFAULT_COMMAND_IN_TOPIC = "~/cmd_vel";
 constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
 constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
+constexpr auto DEFAULT_SET_ODOM_SERVICE = "~/set_odometry";
 }  // namespace
 
 namespace omni_wheel_drive_controller
@@ -206,6 +207,12 @@ controller_interface::CallbackReturn OmniWheelDriveController::on_configure(
   transform_.header.frame_id = odom_frame_id;
   transform_.child_frame_id = base_frame_id;
 
+  set_odom_service_ = get_node()->create_service<control_msgs::srv::SetOdometry>(
+    DEFAULT_SET_ODOM_SERVICE,
+    std::bind(
+      &OmniWheelDriveController::set_odometry, this, std::placeholders::_1, std::placeholders::_2,
+      std::placeholders::_3));
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -317,43 +324,59 @@ controller_interface::return_type OmniWheelDriveController::update_and_write_com
     return controller_interface::return_type::OK;
   }
 
-  // Update odometry
   bool odometry_updated = false;
-  if (params_.open_loop)
+
+  // check if odometry set or reset was requested by non-RT thread
+  if (set_odom_requested_.load())
   {
-    odometry_updated = odometry_.updateOpenLoop(
-      reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2], time);
+    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);
+      odometry_updated = true;
+      set_odom_requested_.store(false);
+    }
   }
   else
   {
-    std::vector<double> wheels_feedback(registered_wheel_handles_.size());  // [rads]
-    for (size_t i = 0; i < static_cast<size_t>(registered_wheel_handles_.size()); ++i)
+    // Update odometry
+    if (params_.open_loop)
+    {
+      odometry_updated = odometry_.updateOpenLoop(
+        reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2], time);
+    }
+    else
     {
-      // Get wheel feedback
-      const std::optional<double> wheel_feedback_op =
-        registered_wheel_handles_[i].feedback.value().get().get_optional();
-      if (!wheel_feedback_op.has_value())
+      std::vector<double> wheels_feedback(registered_wheel_handles_.size());  // [rads]
+      for (size_t i = 0; i < static_cast<size_t>(registered_wheel_handles_.size()); ++i)
       {
-        RCLCPP_DEBUG(logger, "Unable to retrieve data from [%zu] wheel feedback!", i);
-        return controller_interface::return_type::OK;
-      }
-      const double wheel_feedback = wheel_feedback_op.value();
+        // Get wheel feedback
+        const std::optional<double> wheel_feedback_op =
+          registered_wheel_handles_[i].feedback.value().get().get_optional();
+        if (!wheel_feedback_op.has_value())
+        {
+          RCLCPP_DEBUG(logger, "Unable to retrieve data from [%zu] wheel feedback!", i);
+          return controller_interface::return_type::OK;
+        }
+        const double wheel_feedback = wheel_feedback_op.value();
+
+        if (std::isnan(wheel_feedback))
+        {
+          RCLCPP_ERROR(logger, "The wheel %s is invalid for index [%zu]", feedback_type(), i);
+          return controller_interface::return_type::ERROR;
+        }
 
-      if (std::isnan(wheel_feedback))
+        wheels_feedback[i] = wheel_feedback;
+      }
+      if (params_.position_feedback)
       {
-        RCLCPP_ERROR(logger, "The wheel %s is invalid for index [%zu]", feedback_type(), i);
-        return controller_interface::return_type::ERROR;
+        odometry_updated = odometry_.updateFromPos(wheels_feedback, time);
+      }
+      else
+      {
+        odometry_updated = odometry_.updateFromVel(wheels_feedback, time);
       }
-
-      wheels_feedback[i] = wheel_feedback;
-    }
-    if (params_.position_feedback)
-    {
-      odometry_updated = odometry_.updateFromPos(wheels_feedback, time);
-    }
-    else
-    {
-      odometry_updated = odometry_.updateFromVel(wheels_feedback, time);
     }
   }
 
@@ -436,9 +459,31 @@ void OmniWheelDriveController::compute_and_set_wheel_velocities()
     logger, !set_command_result, "Unable to set the command to one of the command handles!");
 }
 
+void OmniWheelDriveController::set_odometry(
+  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)
+{
+  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_requested_.store(true);
+
+  res->success = true;
+  res->message = "Odometry set request accepted";
+}
+
 bool OmniWheelDriveController::reset()
 {
-  odometry_.resetOdometry();
+  odometry_.setOdometry(0.0, 0.0, 0.0);
 
   reset_buffers();
 

omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp

@@ -819,6 +819,70 @@ TEST_F(OmniWheelDriveControllerTest, 5_wheel_test)
   executor.cancel();
 }
 
+TEST_F(OmniWheelDriveControllerTest, odometry_set_service)
+{
+  // 0. Initialize and activate
+  ASSERT_EQ(InitController(), controller_interface::return_type::OK);
+
+  // chained mode needed to write directly to reference interfaces
+  controller_->set_chained_mode(true);
+
+  auto state = controller_->get_node()->configure();
+  ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
+  assignResourcesPosFeedback();
+
+  state = controller_->get_node()->activate();
+  ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
+
+  const double dt = 0.01;  // 100 Hz
+  rclcpp::Time test_time(0, 0, RCL_ROS_TIME);
+  const rclcpp::Duration period = rclcpp::Duration::from_seconds(dt);
+
+  // simulate movement
+  auto move_robot = [&](double vx, double vy, double wz)
+  {
+    controller_->reference_interfaces_[0] = vx;  // linear x
+    controller_->reference_interfaces_[1] = vy;  // linear y
+    controller_->reference_interfaces_[2] = wz;  // angular z
+
+    ASSERT_EQ(controller_->update(test_time, period), controller_interface::return_type::OK);
+
+    for (size_t i = 0; i < wheels_pos_states_.size(); ++i)
+    {
+      double velocity_command = command_itfs_[i]->get_optional().value();
+      wheels_pos_states_[i] += velocity_command * dt;
+    }
+    test_time += period;
+  };
+
+  // 1. Move the robot forward in X
+  for (int i = 0; i < 5; ++i) move_robot(1.0, 0.0, 0.0);
+  ASSERT_GT(controller_->odometry_.getX(), 0.0);
+
+  // 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;
+  set_request->y = -2.0;
+  set_request->yaw = 1.57079632679;  // 90 degrees
+  controller_->set_odometry(nullptr, set_request, set_response);
+  EXPECT_TRUE(set_response->success);
+
+  controller_->update(test_time, period);
+  EXPECT_NEAR(controller_->odometry_.getX(), 5.0, 1e-6);
+  EXPECT_NEAR(controller_->odometry_.getY(), -2.0, 1e-6);
+  EXPECT_NEAR(controller_->odometry_.getHeading(), 1.57079632679, 1e-5);
+
+  // 3. Move again to ensure it still works
+  double start_y = controller_->odometry_.getY();
+  for (int i = 0; i < 5; ++i) move_robot(1.0, 0.0, 0.0);  // we are facing +Y now
+  EXPECT_GT(controller_->odometry_.getY(), start_y);
+
+  // 4. Cleanup
+  state = controller_->get_node()->deactivate();
+  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp

@@ -62,6 +62,7 @@ class TestableOmniWheelDriveController
   FRIEND_TEST(OmniWheelDriveControllerTest, 3_wheel_rot_test);
   FRIEND_TEST(OmniWheelDriveControllerTest, 4_wheel_rot_test);
   FRIEND_TEST(OmniWheelDriveControllerTest, 5_wheel_test);
+  FRIEND_TEST(OmniWheelDriveControllerTest, odometry_set_service);
 
   /**
    * @brief wait_for_twist block until a new twist is received.