feat: add ROS service to reset odometry (#479)

* feat: Implement a ROS service '/reset_odometry' that allows users to
reset the internal state of the odometry and map.

* This feature was requested in the closed issue #478.

Author: qza36

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -73,5 +73,16 @@ KissICP::Vector3dVectorTuple KissICP::Voxelize(const std::vector<Eigen::Vector3d
     const auto source = kiss_icp::VoxelDownsample(frame_downsample, voxel_size * 1.5);
     return {source, frame_downsample};
 }
+void KissICP::Reset() {
+    last_pose_ = Sophus::SE3d();
+    last_delta_ = Sophus::SE3d();
+
+    // Clear the local map
+    local_map_.Clear();
+
+    // Reset adaptive threshold (it will start fresh)
+    adaptive_threshold_ =
+        AdaptiveThreshold(config_.initial_threshold, config_.min_motion_th, config_.max_range);
+}
 
 }  // namespace kiss_icp::pipeline

cpp/kiss_icp/pipeline/KissICP.hpp

@@ -82,6 +82,7 @@ class KissICP {
 
     const Sophus::SE3d &delta() const { return last_delta_; }
     Sophus::SE3d &delta() { return last_delta_; }
+    void Reset();
 
 private:
     Sophus::SE3d last_pose_;

ros/CMakeLists.txt

@@ -47,6 +47,7 @@ find_package(rcutils REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(std_srvs REQUIRED)
 
 # ROS 2 node
 add_library(odometry_component SHARED src/OdometryServer.cpp)
@@ -62,6 +63,7 @@ ament_target_dependencies(
   rcutils
   sensor_msgs
   std_msgs
+  std_srvs
   tf2_ros)
 
 rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE kiss_icp_node)

ros/package.xml

@@ -42,6 +42,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>std_srvs</depend>
 
   <depend>eigen</depend>
   <depend>sophus</depend>

ros/src/OdometryServer.cpp

@@ -44,7 +44,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/string.hpp>
-
+#include <std_srvs/srv/empty.hpp>
 namespace {
 Sophus::SE3d LookupTransform(const std::string &target_frame,
                              const std::string &source_frame,
@@ -98,6 +98,10 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
     tf2_buffer_->setUsingDedicatedThread(true);
     tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_buffer_);
+    // Initialize service servers
+    reset_service_ = create_service<std_srvs::srv::Empty>(
+        "kiss/reset", std::bind(&OdometryServer::ResetService, this, std::placeholders::_1,
+                                std::placeholders::_2));
 
     RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized");
 }
@@ -223,11 +227,20 @@ void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> &frame,
 
     frame_publisher_->publish(std::move(EigenToPointCloud2(frame, header)));
     kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, header)));
-
     auto local_map_header = header;
     local_map_header.frame_id = lidar_odom_frame_;
     map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, local_map_header)));
 }
+void OdometryServer::ResetService(
+    [[maybe_unused]] const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+    [[maybe_unused]] std::shared_ptr<std_srvs::srv::Empty::Response> response) {
+    RCLCPP_INFO(this->get_logger(), "Resetting KISS-ICP map and odometry");
+
+    // Reset the KISS-ICP pipeline
+    kiss_icp_->Reset();
+
+    RCLCPP_INFO(this->get_logger(), "KISS-ICP reset completed successfully");
+}
 }  // namespace kiss_icp_ros
 
 #include "rclcpp_components/register_node_macro.hpp"

ros/src/OdometryServer.hpp

@@ -34,6 +34,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/header.hpp>
+#include <std_srvs/srv/empty.hpp>
 #include <string>
 
 namespace kiss_icp_ros {
@@ -59,6 +60,8 @@ class OdometryServer : public rclcpp::Node {
     void PublishClouds(const std::vector<Eigen::Vector3d> &frame,
                        const std::vector<Eigen::Vector3d> &keypoints,
                        const std_msgs::msg::Header &header);
+    void ResetService(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+                      std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
 private:
     /// Tools for broadcasting TFs.
@@ -78,6 +81,9 @@ class OdometryServer : public rclcpp::Node {
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr kpoints_publisher_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr map_publisher_;
 
+    /// Service servers.
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_service_;
+
     /// KISS-ICP
     std::unique_ptr<kiss_icp::pipeline::KissICP> kiss_icp_;