From 1ff48103345bc6913ce001be9bdb1f5fa27ffde5 Mon Sep 17 00:00:00 2001
From: pimpest <82343504+pimpest@users.noreply.github.com>
Date: Sun, 8 Feb 2026 01:58:33 +0100
Subject: [PATCH] initialpose topic sets now position of base_link allowing
 relocation

---
 toid_control/launch/toid.launch.py           |  2 +-
 toid_control/params/toid_general_params.yaml |  2 +-
 toid_odometry/src/odometry.cpp               | 47 +++++++++++++++++---
 3 files changed, 44 insertions(+), 7 deletions(-)

diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py
index 0a733f9..90060f8 100644
--- a/toid_control/launch/toid.launch.py
+++ b/toid_control/launch/toid.launch.py
@@ -47,7 +47,7 @@ def generate_launch_description():
         package='toid_odometry',
         executable='toid_odometry',
         name='map_to_odom_broadcaster',
-        arguments={'use_mock': use_mock},
+        parameters=[{'mock_odom': use_mock}],
         condition=IfCondition(LaunchConfiguration('visualize'))
     )
 
diff --git a/toid_control/params/toid_general_params.yaml b/toid_control/params/toid_general_params.yaml
index 78b6d8f..6a6cb64 100644
--- a/toid_control/params/toid_general_params.yaml
+++ b/toid_control/params/toid_general_params.yaml
@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200
+    update_rate: 50
 
     diffdrive_controller:
       type: diff_drive_controller/DiffDriveController
diff --git a/toid_odometry/src/odometry.cpp b/toid_odometry/src/odometry.cpp
index 55d6d88..266b004 100644
--- a/toid_odometry/src/odometry.cpp
+++ b/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_;