feat: added Reset initial pose through ros service

- clears local map
- clears path (trajectory)

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -107,4 +107,12 @@ bool KissICP::HasMoved() {
     return motion > 5.0 * config_.min_motion_th;
 }
 
+void KissICP::Reset(Eigen::Quaterniond quaternion,Eigen::Vector3d translation )
+{   
+    poses_.clear();
+    Sophus::SE3d predpred(quaternion, translation);
+    poses_.push_back(predpred);
+    local_map_.Clear();
+}
+
 }  // namespace kiss_icp::pipeline

cpp/kiss_icp/pipeline/KissICP.hpp

@@ -67,6 +67,7 @@ class KissICP {
     double GetAdaptiveThreshold();
     Sophus::SE3d GetPredictionModel() const;
     bool HasMoved();
+    void Reset(Eigen::Quaterniond, Eigen::Vector3d);
 
 public:
     // Extra C++ API to facilitate ROS debugging

ros/CMakeLists.txt

@@ -51,7 +51,10 @@ if("$ENV{ROS_VERSION}" STREQUAL "1")
                rosbag
                std_msgs
                tf2
-               tf2_ros)
+               tf2_ros
+               message_generation)
+  add_service_files(FILES  set_pose.srv)
+  generate_messages(DEPENDENCIES std_msgs) 
   catkin_package()
 
   # ROS 1 node

ros/package.xml

@@ -43,7 +43,8 @@
   <depend condition="$ROS_VERSION == 2">rclcpp</depend>
   <depend condition="$ROS_VERSION == 2">rclcpp_components</depend>
   <exec_depend condition="$ROS_VERSION == 2">ros2launch</exec_depend>
-
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
   <!-- ROS1/2 dependencies -->
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>

ros/ros1/OdometryServer.cpp

@@ -74,6 +74,8 @@ OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle
     // Initialize subscribers
     pointcloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("pointcloud_topic", queue_size_,
                                                               &OdometryServer::RegisterFrame, this);
+    service = nh_.advertiseService("reset_pose", &OdometryServer::reset_pose, this);
+
 
     // Initialize publishers
     odom_publisher_ = pnh_.advertise<nav_msgs::Odometry>("odometry", queue_size_);
@@ -172,6 +174,24 @@ void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg
     local_map_header.frame_id = odom_frame_;
     map_publisher_.publish(*std::move(EigenToPointCloud2(odometry_.LocalMap(), local_map_header)));
 }
+
+bool OdometryServer::reset_pose(kiss_icp::set_pose::Request &req,kiss_icp::set_pose::Response &res)
+    {
+        Sophus::SE3d se3d_transform;
+        se3d_transform.setQuaternion(Eigen::Quaterniond(Eigen::AngleAxisd(req.R, Eigen::Vector3d::UnitX()) *
+                                                   Eigen::AngleAxisd(req.P, Eigen::Vector3d::UnitY()) *
+                                                   Eigen::AngleAxisd(req.Y, Eigen::Vector3d::UnitZ())));
+
+        Eigen::Quaterniond quaternion = se3d_transform.unit_quaternion();
+
+        Eigen::Vector3d  vector(req.x, req.y, req.z);
+
+        odometry_.Reset(quaternion, vector);
+        path_msg_.poses.clear();
+        res.done = true;
+        return true;
+    }
+
 }  // namespace kiss_icp_ros
 
 int main(int argc, char **argv) {

ros/ros1/OdometryServer.hpp

@@ -30,6 +30,7 @@
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include "kiss_icp/set_pose.h"
 
 namespace kiss_icp_ros {
 
@@ -41,6 +42,8 @@ class OdometryServer {
 private:
     /// Register new frame
     void RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg);
+    bool reset_pose(kiss_icp::set_pose::Request &req,
+                    kiss_icp::set_pose::Response &res);
 
     /// Ros node stuff
     ros::NodeHandle nh_;
@@ -62,6 +65,7 @@ class OdometryServer {
     ros::Publisher frame_publisher_;
     ros::Publisher kpoints_publisher_;
     ros::Publisher map_publisher_;
+    ros::ServiceServer service;
 
     /// KISS-ICP
     kiss_icp::pipeline::KissICP odometry_;

ros/srv/set_pose.srv

@@ -0,0 +1,10 @@
+# Translation XYZ [meters]
+float32 x
+float32 y
+float32 z
+# Orientation RPY [radians]
+float32 R
+float32 P
+float32 Y
+---
+bool done