initialpose topic sets now position of base_link allowing relocation

toid_odometry/src/odometry.cpp

@@ -1,7 +1,13 @@
 #include <iostream>
 #include <chrono>
 #include <boost/asio.hpp>
+#include <memory>
+#include <tf2/exceptions.hpp>
+#include <tf2/time.hpp>
+#include <tf2_ros/transform_listener.hpp>
 
+#include "geometry_msgs/msg/transform.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "toid_odometry/msgs.h"
@@ -10,6 +16,7 @@
 #include "toid_msgs/srv/send_double.hpp"
 #include "std_srvs/srv/empty.hpp"
 
+#include "tf2_ros/buffer.hpp"
 #include "tf2_ros/transform_broadcaster.hpp"
 #include "tf2_ros/static_transform_broadcaster.hpp"
 #include "tf2/LinearMath/Transform.hpp"
@@ -41,6 +48,10 @@ public:
 
         tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
         tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
+
+        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
         geometry_msgs::msg::TransformStamped t;
         t.header.stamp = this->get_clock()->now();
         t.header.frame_id = map_frame_id_;
@@ -74,17 +85,41 @@ public:
 private:
 
     void set_pose(const geometry_msgs::msg::PoseWithCovarianceStamped &pose) {
+
         geometry_msgs::msg::TransformStamped t;
 
+        try {
+            geometry_msgs::msg::TransformStamped base = tf_buffer_->lookupTransform(
+                odometry_frame_id_, 
+                base_frame_id_, 
+                tf2::TimePointZero
+            );
+
+            t.transform.rotation = pose.pose.pose.orientation;
+            auto[x,y,z] =pose.pose.pose.position;
+            t.transform.translation.x = x;
+            t.transform.translation.y = y;
+            t.transform.translation.z = z;
+
+            tf2::Transform map_target;
+            tf2::fromMsg(t.transform, map_target);
+
+            tf2::Transform odom_robot;
+            tf2::fromMsg(base.transform, odom_robot);
+
+            tf2::Transform map_odom;
+            map_odom = map_target * odom_robot.inverse();
+            t.transform = tf2::toMsg(map_odom);
+        }
+        catch (const tf2::TransformException &e) {
+            RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+            return;
+        }
+
         t.header.stamp = this->get_clock()->now();
         t.header.frame_id = map_frame_id_;
         t.child_frame_id = odometry_frame_id_;
 
-        t.transform.rotation = pose.pose.pose.orientation;
-        auto[x,y,z] =pose.pose.pose.position;
-        t.transform.translation.x = x;
-        t.transform.translation.y = y;
-        t.transform.translation.z = z;
 
         RCLCPP_INFO(this->get_logger(), "Setting new initial_pose");
 
@@ -195,6 +230,8 @@ private:
 
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
     std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
     rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
     rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
     rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;