set and reset odometry service additions - diff drive controller

diff_drive_controller/CMakeLists.txt

@@ -6,6 +6,7 @@ set_compiler_options()
 export_windows_symbols()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
   control_toolbox
   controller_interface
   generate_parameter_library
@@ -17,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp_lifecycle
   rcpputils
   realtime_tools
+  std_srvs
   tf2
   tf2_msgs
 )
@@ -60,7 +62,8 @@ target_link_libraries(diff_drive_controller
     tf2::tf2
     ${tf2_msgs_TARGETS}
     ${geometry_msgs_TARGETS}
-    ${nav_msgs_TARGETS})
+    ${nav_msgs_TARGETS}
+    ${std_srvs_TARGETS})
 pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml)
 
 if(BUILD_TESTING)

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -19,12 +19,14 @@
 #ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
 #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
 
+#include <atomic>
 #include <chrono>
 #include <memory>
 #include <queue>
 #include <string>
 #include <vector>
 
+#include "control_msgs/srv/set_odometry.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "diff_drive_controller/odometry.hpp"
 #include "diff_drive_controller/speed_limiter.hpp"
@@ -34,6 +36,7 @@
 #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 "tf2_msgs/msg/tf_message.hpp"
 
 // auto-generated by generate_parameter_library
@@ -76,6 +79,15 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   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);
+  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:
   bool on_set_chained_mode(bool chained_mode) override;
 
@@ -147,6 +159,14 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
   rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
 
+  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_;
+
   bool reset();
   void halt();
 

diff_drive_controller/include/diff_drive_controller/odometry.hpp

@@ -55,6 +55,7 @@ class Odometry
   bool update_from_pos(double left_pos, double right_pos, double dt);
   bool update_from_vel(double left_vel, double right_vel, double dt);
   bool try_update_open_loop(double linear_vel, double angular_vel, double dt);
+  void setOdometry(double x, double y, double heading);
   void resetOdometry();
 
   double getX() const { return x_; }

diff_drive_controller/package.xml

@@ -26,6 +26,7 @@
   <build_depend>ros2_control_cmake</build_depend>
 
   <depend>backward_ros</depend>
+  <depend>control_msgs</depend>
   <depend>control_toolbox</depend>
   <depend>controller_interface</depend>
   <depend>geometry_msgs</depend>
@@ -36,6 +37,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
   <depend>realtime_tools</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_msgs</depend>
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -28,13 +28,16 @@
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/logging.hpp"
 #include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2/impl/utils.hpp"
 
 namespace
 {
 constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel";
 constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out";
 constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
 constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
+constexpr auto DEFAULT_SET_ODOM_SERVICE = "~/set_odometry";
+constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry";
 }  // namespace
 
 namespace diff_drive_controller
@@ -161,56 +164,75 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
   const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
   const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;
 
-  // 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_request_.load())
   {
-    odometry_updated =
-      odometry_.try_update_open_loop(linear_command, angular_command, period.seconds());
+    odometry_.setOdometry(
+      requested_odom_params_.x, requested_odom_params_.y, requested_odom_params_.yaw);
+    set_odom_request_.store(false);
+    odometry_updated = true;
+  }
+  else if (reset_odom_request_.load())
+  {
+    odometry_.resetOdometry();
+    reset_odom_request_.store(false);
+    odometry_updated = true;
   }
   else
   {
-    double left_feedback_mean = 0.0;
-    double right_feedback_mean = 0.0;
-    for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
+    // Update odometry
+    if (params_.open_loop)
     {
-      const auto left_feedback_op =
-        registered_left_wheel_handles_[index].feedback.value().get().get_optional();
-      const auto right_feedback_op =
-        registered_right_wheel_handles_[index].feedback.value().get().get_optional();
-
-      if (!left_feedback_op.has_value() || !right_feedback_op.has_value())
+      odometry_updated =
+        odometry_.try_update_open_loop(linear_command, angular_command, period.seconds());
+    }
+    else
+    {
+      double left_feedback_mean = 0.0;
+      double right_feedback_mean = 0.0;
+      for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
       {
-        RCLCPP_DEBUG(logger, "Unable to retrieve the data from the left or right wheels feedback!");
-        return controller_interface::return_type::OK;
-      }
+        const auto left_feedback_op =
+          registered_left_wheel_handles_[index].feedback.value().get().get_optional();
+        const auto right_feedback_op =
+          registered_right_wheel_handles_[index].feedback.value().get().get_optional();
 
-      const double left_feedback = left_feedback_op.value();
-      const double right_feedback = right_feedback_op.value();
+        if (!left_feedback_op.has_value() || !right_feedback_op.has_value())
+        {
+          RCLCPP_DEBUG(
+            logger, "Unable to retrieve the data from the left or right wheels feedback!");
+          return controller_interface::return_type::OK;
+        }
 
-      if (std::isnan(left_feedback) || std::isnan(right_feedback))
-      {
-        RCLCPP_ERROR(
-          logger, "Either the left or right wheel %s is invalid for index [%zu]", feedback_type(),
-          index);
-        return controller_interface::return_type::ERROR;
-      }
+        const double left_feedback = left_feedback_op.value();
+        const double right_feedback = right_feedback_op.value();
 
-      left_feedback_mean += left_feedback;
-      right_feedback_mean += right_feedback;
-    }
-    left_feedback_mean /= static_cast<double>(wheels_per_side_);
-    right_feedback_mean /= static_cast<double>(wheels_per_side_);
+        if (std::isnan(left_feedback) || std::isnan(right_feedback))
+        {
+          RCLCPP_ERROR(
+            logger, "Either the left or right wheel %s is invalid for index [%zu]", feedback_type(),
+            index);
+          return controller_interface::return_type::ERROR;
+        }
+
+        left_feedback_mean += left_feedback;
+        right_feedback_mean += right_feedback;
+      }
+      left_feedback_mean /= static_cast<double>(wheels_per_side_);
+      right_feedback_mean /= static_cast<double>(wheels_per_side_);
 
-    if (params_.position_feedback)
-    {
-      odometry_updated =
-        odometry_.update_from_pos(left_feedback_mean, right_feedback_mean, period.seconds());
-    }
-    else
-    {
-      odometry_updated =
-        odometry_.update_from_vel(left_feedback_mean, right_feedback_mean, period.seconds());
+      if (params_.position_feedback)
+      {
+        odometry_updated =
+          odometry_.update_from_pos(left_feedback_mean, right_feedback_mean, period.seconds());
+      }
+      else
+      {
+        odometry_updated =
+          odometry_.update_from_vel(left_feedback_mean, right_feedback_mean, period.seconds());
+      }
     }
   }
 
@@ -492,6 +514,16 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   odometry_transform_message_.transforms.front().header.frame_id = odom_frame_id;
   odometry_transform_message_.transforms.front().child_frame_id = base_frame_id;
 
+  // Create odometry set & reset services
+  set_odom_service_ = get_node()->create_service<control_msgs::srv::SetOdometry>(
+    DEFAULT_SET_ODOM_SERVICE, std::bind(
+                                &DiffDriveController::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(
+                                  &DiffDriveController::reset_odometry, this, std::placeholders::_1,
+                                  std::placeholders::_2, std::placeholders::_3));
+
   previous_update_timestamp_ = get_node()->get_clock()->now();
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -556,6 +588,29 @@ controller_interface::CallbackReturn DiffDriveController::on_error(const rclcpp_
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
+void DiffDriveController::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)
+{
+  // update the flag for setting odom thread-safely 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";
+}
+void DiffDriveController::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*/)
+{
+  // update the flag for resetting odom thread-safely in the control loop
+  reset_odom_request_.store(true);
+  RCLCPP_INFO(get_node()->get_logger(), "Odometry reset requested");
+}
+
 bool DiffDriveController::reset()
 {
   odometry_.resetOdometry();

diff_drive_controller/src/odometry.cpp

@@ -167,11 +167,19 @@ bool Odometry::try_update_open_loop(double linear_vel, double angular_vel, doubl
   return true;
 }
 
+void Odometry::setOdometry(double x, double y, double heading)
+{
+  x_ = x;
+  y_ = y;
+  heading_ = heading;
+  resetAccumulators();
+}
 void Odometry::resetOdometry()
 {
   x_ = 0.0;
   y_ = 0.0;
   heading_ = 0.0;
+  resetAccumulators();
 }
 
 void Odometry::setWheelParams(