Merge pull request #1131 from ROBOTIS-GIT/feature-add-odom-reset

GyuH13 · web-flow · commit da785b7201d3 · 2025-12-15T21:08:37.000+09:00
Fix the Reset Trigger Service
diff --git a/ISSUE_TEMPLATE.md b/ISSUE_TEMPLATE.md
@@ -1,4 +1,4 @@
-ISSUE TEMPLATE ver. 0.4.2
+ISSUE TEMPLATE ver. 0.4.3
 
 **Please search for similar issues in the repository before submit an issue ticket.**
 
@@ -11,6 +11,7 @@ ISSUE TEMPLATE ver. 0.4.2
 
     - [ ] Raspberry Pi 3 B+
     - [ ] Raspberry Pi 4 (specify RAM size: 2/4/8GB)
+    - [ ] Raspberry Pi 5 (specify RAM size: 2/4/8GB)
     - [ ] other (specify the product)
 
 3. Which OS is installed on the TurtleBot3 SBC?
@@ -19,6 +20,7 @@ ISSUE TEMPLATE ver. 0.4.2
     - [ ] Ubuntu preinstalled server 18.04
     - [ ] Ubuntu preinstalled server 20.04
     - [ ] Ubuntu preinstalled server 22.04
+    - [ ] Ubuntu preinstalled server 24.04
     - [ ] other (specify the OS)
 
 4. Which OS is installed on Remote PC?
@@ -27,6 +29,7 @@ ISSUE TEMPLATE ver. 0.4.2
     - [ ] Ubuntu 18.04 LTS (Bionic Beaver)
     - [ ] Ubuntu 20.04 LTS (Focal Fossa)
     - [ ] Ubuntu 22.04 LTS (Jammy Jellyfish)
+    - [ ] Ubuntu 24.04 LTS (Noble Numbat)
     - [ ] other (specify the OS)
 
 5. Which ROS version do you use?
@@ -39,6 +42,7 @@ ISSUE TEMPLATE ver. 0.4.2
     - [ ] ROS2 Foxy Fitzroy
     - [ ] ROS2 Galactic Geochelone
     - [ ] ROS2 Humble Hawksbill
+    - [ ] ROS2 Jazzy Jellyfish
     - [ ] ROS2 Rolling Ridley
 
 6. Please describe the issue.
diff --git a/README.md b/README.md
@@ -1,8 +1,8 @@
 # TurtleBot3
 <img src="https://raw.githubusercontent.com/ROBOTIS-GIT/emanual/master/assets/images/platform/turtlebot3/logo_turtlebot3.png" width="300">
 
-- Active Branches: noetic, humble, jazzy, main(rolling)
-- Legacy Branches: *-devel
+- Active Branches: humble, jazzy, main(rolling)
+- Legacy Branches: *-devel, noetic
 
 ## Open Source Projects Related to TurtleBot3
 - [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3)
@@ -17,6 +17,7 @@
 - [turtlebot3_home_service_challenge](https://github.com/ROBOTIS-GIT/turtlebot3_home_service_challenge)
 - [hls_lfcd_lds_driver](https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver)
 - [ld08_driver](https://github.com/ROBOTIS-GIT/ld08_driver)
+- [coin_d4_driver](https://github.com/ROBOTIS-GIT/coin_d4_driver)
 - [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator)
 - [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK)
 - [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware)
diff --git a/turtlebot3/CHANGELOG.rst b/turtlebot3/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package turtlebot3
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* Fixed an issue where the reset trigger did not work correctly
+* Contributors: Hyungyu Kim
+
 2.3.5 (2025-12-09)
 ------------------
 * Supported Pi Camera in Docker containers for TurtleBot3 with Humble and Jazzy
diff --git a/turtlebot3/package.xml b/turtlebot3/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     ROS 2 packages for TurtleBot3
   </description>
diff --git a/turtlebot3_bringup/CHANGELOG.rst b/turtlebot3_bringup/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_bringup
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* None
+
 2.3.5 (2025-12-09)
 ------------------
 * None
diff --git a/turtlebot3_bringup/package.xml b/turtlebot3_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_bringup</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     ROS 2 launch scripts for starting the TurtleBot3
   </description>
diff --git a/turtlebot3_cartographer/CHANGELOG.rst b/turtlebot3_cartographer/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_cartographer
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* None
+
 2.3.5 (2025-12-09)
 ------------------
 * None
diff --git a/turtlebot3_cartographer/package.xml b/turtlebot3_cartographer/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_cartographer</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     ROS 2 launch scripts for cartographer
   </description>
diff --git a/turtlebot3_description/CHANGELOG.rst b/turtlebot3_description/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* None
+
 2.3.5 (2025-12-09)
 ------------------
 * None
diff --git a/turtlebot3_description/package.xml b/turtlebot3_description/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_description</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     3D models of the TurtleBot3 for simulation and visualization
   </description>
diff --git a/turtlebot3_example/CHANGELOG.rst b/turtlebot3_example/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_example
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* None
+
 2.3.5 (2025-12-09)
 ------------------
 * None
diff --git a/turtlebot3_example/package.xml b/turtlebot3_example/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_example</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     This package provides four basic examples for TurtleBot3 (i.e., interactive marker, object detection, patrol and position control).
   </description>
diff --git a/turtlebot3_example/setup.py b/turtlebot3_example/setup.py
@@ -5,7 +5,7 @@
 
 setup(
     name=package_name,
-    version='2.3.5',
+    version='2.3.6',
     packages=find_packages(),
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
diff --git a/turtlebot3_navigation2/CHANGELOG.rst b/turtlebot3_navigation2/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_navigation2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* None
+
 2.3.5 (2025-12-09)
 ------------------
 * None
diff --git a/turtlebot3_navigation2/package.xml b/turtlebot3_navigation2/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_navigation2</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     ROS 2 launch scripts for navigation2
   </description>
diff --git a/turtlebot3_node/CHANGELOG.rst b/turtlebot3_node/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package turtlebot3_node
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* Fixed an issue where the reset trigger did not work correctly
+* Contributors: Hyungyu Kim
+
 2.3.5 (2025-12-09)
 ------------------
 * None
diff --git a/turtlebot3_node/include/turtlebot3_node/devices/reset.hpp b/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
diff --git a/turtlebot3_node/include/turtlebot3_node/odometry.hpp b/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_;
diff --git a/turtlebot3_node/package.xml b/turtlebot3_node/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_node</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     TurtleBot3 driver node that include diff drive controller, odometry and tf node
   </description>
diff --git a/turtlebot3_node/src/devices/reset.cpp b/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(
diff --git a/turtlebot3_node/src/odometry.cpp b/turtlebot3_node/src/odometry.cpp
@@ -118,6 +118,14 @@ 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 +304,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";
+}
diff --git a/turtlebot3_teleop/CHANGELOG.rst b/turtlebot3_teleop/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package turtlebot3_teleop
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.3.6 (2025-12-15)
+------------------
+* None
+
 2.3.5 (2025-12-09)
 ------------------
 * None
diff --git a/turtlebot3_teleop/package.xml b/turtlebot3_teleop/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>turtlebot3_teleop</name>
-  <version>2.3.5</version>
+  <version>2.3.6</version>
   <description>
     Teleoperation node using keyboard for TurtleBot3.
   </description>
diff --git a/turtlebot3_teleop/setup.py b/turtlebot3_teleop/setup.py
@@ -5,7 +5,7 @@
 
 setup(
     name=package_name,
-    version='2.3.5',
+    version='2.3.6',
     packages=find_packages(exclude=[]),
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
