Fix the Reset Trigger Service

turtlebot3_node/include/turtlebot3_node/devices/reset.hpp

@@ -46,6 +46,7 @@ class Reset : public Devices
 
 private:
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr reset_odom_client_;
 };
 }  // namespace devices
 }  // namespace turtlebot3

turtlebot3_node/include/turtlebot3_node/odometry.hpp

@@ -33,6 +33,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 
 namespace robotis
@@ -51,6 +52,10 @@ class Odometry
 private:
   bool calculate_odometry(const rclcpp::Duration & duration);
 
+  void reset_odometry_callback(
+    const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+    std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+
   void update_imu(const std::shared_ptr<sensor_msgs::msg::Imu const> & imu);
   void update_joint_state(const std::shared_ptr<sensor_msgs::msg::JointState const> & joint_state);
 
@@ -67,6 +72,7 @@ class Odometry
 
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_odom_srv_;
 
   std::shared_ptr<
     message_filters::Subscriber<sensor_msgs::msg::JointState>> msg_ftr_joint_state_sub_;

turtlebot3_node/src/devices/reset.cpp

@@ -37,25 +37,46 @@ Reset::Reset(
       this->command(static_cast<void *>(request.get()), static_cast<void *>(response.get()));
     }
   );
+
+  reset_odom_client_ = nh_->create_client<std_srvs::srv::Trigger>("reset_odometry");
 }
 
 void Reset::command(const void * request, void * response)
 {
   (void) request;
 
   std_srvs::srv::Trigger::Response * res = (std_srvs::srv::Trigger::Response *)response;
+  std::string result_msg;
 
   uint8_t reset = 1;
-
-  res->success = dxl_sdk_wrapper_->set_data_to_device(
+  dxl_sdk_wrapper_->set_data_to_device(
     extern_control_table.imu_re_calibration.addr,
     extern_control_table.imu_re_calibration.length,
     &reset,
-    &res->message);
+    &result_msg);
 
   RCLCPP_INFO(nh_->get_logger(), "Start Calibration of Gyro");
   rclcpp::sleep_for(std::chrono::seconds(5));
   RCLCPP_INFO(nh_->get_logger(), "Calibration End");
+  res->success = true;
+  res->message = "Calibration End, Odom reset requested";
+
+  if (!reset_odom_client_->wait_for_service(std::chrono::seconds(1))) {
+    RCLCPP_WARN(nh_->get_logger(), "reset_odometry service not available");
+    return;
+  }
+
+  auto request_reset_odom = std::make_shared<std_srvs::srv::Trigger::Request>();
+  reset_odom_client_->async_send_request(
+    request_reset_odom,
+    [this](rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture future)
+  {
+    auto response_reset_odom = future.get();
+    RCLCPP_INFO(
+      nh_->get_logger(),
+      "odom reset response: %s",
+      response_reset_odom->success ? "success" : "failed");
+  });
 }
 
 void Reset::request(

turtlebot3_node/src/odometry.cpp

@@ -118,6 +118,10 @@ Odometry::Odometry(
       qos,
       std::bind(&Odometry::joint_state_callback, this, std::placeholders::_1));
   }
+
+  reset_odom_srv_ = nh_->create_service<std_srvs::srv::Trigger>(
+    "reset_odometry",
+    std::bind(&Odometry::reset_odometry_callback, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 void Odometry::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state_msg)
@@ -296,3 +300,14 @@ bool Odometry::calculate_odometry(const rclcpp::Duration & duration)
   last_theta = theta;
   return true;
 }
+
+void Odometry::reset_odometry_callback(
+  const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+  std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+{
+  (void)request;
+  robot_pose_ = {0.0, 0.0, 0.0};
+  robot_vel_ = {0.0, 0.0, 0.0};
+  response->success = true;
+  response->message = "Odometry reset";
+}