feature: add reset service to origin

CMakeLists.txt

@@ -49,6 +49,7 @@ else()
   find_package(ament_cmake REQUIRED)
   find_package(rclcpp REQUIRED CONFIG)
   find_package(std_msgs REQUIRED CONFIG)
+  find_package(std_srvs REQUIRED CONFIG)
   find_package(tf2 REQUIRED CONFIG)
   find_package(tf2_ros REQUIRED CONFIG)
   find_package(sensor_msgs REQUIRED CONFIG)

package.xml

@@ -16,6 +16,7 @@
   <!-- ros dependencies -->
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>sensor_msgs</depend>

rko_lio/core/lio.cpp

@@ -439,4 +439,61 @@ Vector3dVector LIO::register_scan(const Sophus::SE3d& extrinsic_lidar2base,
   transform_points(extrinsic_lidar2base.inverse(), frame);
   return frame;
 }
+
+void LIO::reset_to_origin(bool keep_map, bool keep_rotation) {
+  // NOTE: This function is NOT thread-safe. Caller must ensure proper locking.
+
+  // Store current pose for map transformation
+  const Sophus::SE3d old_pose = lidar_state.pose;
+
+  // Reset pose based on keep_rotation flag
+  // - keep_rotation=true:  new_pose = (R_old, 0) - only translation reset
+  // - keep_rotation=false: new_pose = (I, 0)     - full identity reset
+  if (keep_rotation) {
+    lidar_state.pose.translation().setZero();
+  } else {
+    lidar_state.pose = Sophus::SE3d();
+  }
+
+  // Reset motion states
+  lidar_state.velocity.setZero();
+  lidar_state.angular_velocity.setZero();
+  lidar_state.linear_acceleration.setZero();
+  // Note: lidar_state.time is preserved for IMU synchronization
+
+  // Handle the local map
+  if (keep_map && !map.Empty()) {
+    // Transform map points: p_new = T_new * T_old^-1 * p_old
+    // When keep_rotation=true:  transform = (I, -t_old) - translation only
+    // When keep_rotation=false: transform = T_old^-1   - rotation + translation
+    const Sophus::SE3d transform = lidar_state.pose * old_pose.inverse();
+    Vector3dVector map_points = map.Pointcloud();
+    transform_points(transform, map_points);
+    map.Clear();
+    map.AddPoints(map_points);
+  } else {
+    map.Clear();
+  }
+
+  // Clear trajectory history
+  poses_with_timestamps.clear();
+
+  // Reset IMU integration state (sync with new pose orientation)
+  _imu_local_rotation = lidar_state.pose.so3();
+  // Note: _imu_local_rotation_time is preserved for timing continuity
+  // Note: imu_bias is preserved (assumed already calibrated)
+
+  // Reset body acceleration Kalman filter
+  mean_body_acceleration.setZero();
+  body_acceleration_covariance = Eigen::Matrix3d::Identity();
+
+  // Reset IMU interval statistics
+  interval_stats.reset();
+
+  // Record initial pose in new coordinate frame
+  poses_with_timestamps.emplace_back(lidar_state.time, lidar_state.pose);
+
+  std::cout << "[INFO] Odometry reset to origin. keep_map=" << keep_map
+            << ", keep_rotation=" << keep_rotation << "\n";
+}
 } // namespace rko_lio::core

rko_lio/core/lio.hpp

@@ -138,6 +138,15 @@ class LIO {
   /** Sequence of registered scan poses with corresponding timestamps. */
   std::vector<std::pair<Secondsd, Sophus::SE3d>> poses_with_timestamps;
 
+  /**
+   * Reset odometry to use current pose as the new origin.
+   * @param keep_map If true, transform existing map to new coordinate frame.
+   *                 If false, clear the map entirely.
+   * @param keep_rotation If true, only reset translation (keep current orientation).
+   *                      If false, reset both rotation and translation.
+   */
+  void reset_to_origin(bool keep_map = false, bool keep_rotation = false);
+
 private:
   /**
    * Initialize internal odometry state using the given lidar timestamp.

rko_lio/ros/CMakeLists.txt

@@ -45,6 +45,7 @@ target_link_libraries(
          ${nav_msgs_TARGETS}
          ${sensor_msgs_TARGETS}
          ${std_msgs_TARGETS}
+         ${std_srvs_TARGETS}
          rclcpp::rclcpp
          sensor_msgs::sensor_msgs_library
          tf2::tf2

rko_lio/ros/node.cpp

@@ -165,6 +165,11 @@ Node::Node(const std::string& node_name, const rclcpp::NodeOptions& options) {
 
   registration_thread = std::jthread([this]() { registration_loop(); });
 
+  // Reset odometry service
+  reset_service = node->create_service<std_srvs::srv::Trigger>(
+      "/rko_lio/reset_odometry",
+      std::bind(&Node::reset_odometry_callback, this, std::placeholders::_1, std::placeholders::_2));
+
   RCLCPP_INFO(node->get_logger(), "RKO LIO Node is up!");
 }
 
@@ -173,6 +178,9 @@ void Node::parse_cli_extrinsics() {
     const std::string param_name = "extrinsic_" + name + "2base_quat_xyzw_xyz";
     const std::vector<double> vec = node->declare_parameter<std::vector<double>>(param_name, std::vector<double>{});
 
+    RCLCPP_INFO_STREAM(node->get_logger(), "Parsing " << name << " extrinsic from parameter " << param_name);
+    // print vec value 
+    RCLCPP_INFO_STREAM(node->get_logger(), "vec value: " << Eigen::Map<const Eigen::VectorXd>(vec.data(), vec.size()).transpose());
     if (vec.size() != 7) {
       if (!vec.empty()) {
         RCLCPP_WARN_STREAM(node->get_logger(),
@@ -194,6 +202,7 @@ void Node::parse_cli_extrinsics() {
   const bool imu_ok = parse_extrinsic("imu", extrinsic_imu2base);
   const bool lidar_ok = parse_extrinsic("lidar", extrinsic_lidar2base);
   extrinsics_set = imu_ok && lidar_ok;
+  RCLCPP_INFO_STREAM(node->get_logger(), "extrinsics_set status " << extrinsics_set);
 }
 
 bool Node::check_and_set_extrinsics() {
@@ -432,4 +441,31 @@ void Node::dump_results_to_disk(const std::filesystem::path& results_dir, const
   }
 }
 
+void Node::reset_odometry_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request> /*request*/,
+                                   std::shared_ptr<std_srvs::srv::Trigger::Response> response) {
+  try {
+    // Acquire both locks to safely reset
+    std::scoped_lock lock(buffer_mutex, local_map_mutex);
+
+    // Clear the sensor buffers
+    while (!imu_buffer.empty()) {
+      imu_buffer.pop();
+    }
+    while (!lidar_buffer.empty()) {
+      lidar_buffer.pop();
+    }
+
+    // Reset the LIO algorithm (keep rotation for stability, clear map)
+    lio->reset_to_origin(/*keep_map=*/false, /*keep_rotation=*/true);
+
+    response->success = true;
+    response->message = "Odometry reset to origin successfully.";
+    RCLCPP_INFO(node->get_logger(), "Odometry reset to origin.");
+  } catch (const std::exception& ex) {
+    response->success = false;
+    response->message = std::string("Reset failed: ") + ex.what();
+    RCLCPP_ERROR(node->get_logger(), "Odometry reset failed: %s", ex.what());
+  }
+}
+
 } // namespace rko_lio::ros

rko_lio/ros/node.hpp

@@ -42,6 +42,7 @@
 #include <rclcpp/node_options.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_srvs/srv/trigger.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
@@ -84,6 +85,9 @@ class Node {
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr map_publisher;
   rclcpp::Publisher<geometry_msgs::msg::AccelStamped>::SharedPtr lidar_accel_publisher;
 
+  // reset service
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service;
+
   // multithreading
   std::jthread map_publish_thead;
   core::Secondsd publish_map_after = std::chrono::seconds(1);
@@ -110,6 +114,8 @@ class Node {
   void publish_lidar_accel(const Eigen::Vector3d& acceleration, const core::Secondsd& stamp) const;
   void publish_map_loop();
   void dump_results_to_disk(const std::filesystem::path& results_dir, const std::string& run_name) const;
+  void reset_odometry_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+                               std::shared_ptr<std_srvs::srv::Trigger::Response> response);
 
   ~Node();
   Node(const Node&) = delete;