initialpose topic sets now position of base_link allowing relocation

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'))
     )
 

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

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,11 +85,15 @@ public:
 private:
 
     void set_pose(const geometry_msgs::msg::PoseWithCovarianceStamped &pose) {
+
         geometry_msgs::msg::TransformStamped t;
 
-        t.header.stamp = this->get_clock()->now();
-        t.header.frame_id = map_frame_id_;
-        t.child_frame_id = odometry_frame_id_;
+        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;
@@ -86,6 +101,26 @@ private:
             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_;
+
+
         RCLCPP_INFO(this->get_logger(), "Setting new initial_pose");
 
         tf_static_broadcaster_->sendTransform(t);
@@ -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_;