From d729ce5173bcaf3eeba931711ddb90d8a8b870a7 Mon Sep 17 00:00:00 2001 From: Samuel Toledano Date: Tue, 29 Oct 2024 10:56:35 +0100 Subject: [PATCH] Fix transformations to use correct parents and children frames --- src/message_wrapper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index d0c0b6d..ec91cc2 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1036,7 +1036,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv pose.position.y = first_valid_northing_; pose.position.z = first_valid_altitude_; - fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform); + fillTransform(odom_init_frame_id_, odom_base_frame_id_, pose, transform); tf_broadcaster_->sendTransform(transform); static_tf_broadcaster_->sendTransform(transform); } @@ -1084,7 +1084,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv if (odom_publish_tf_) { // Publish odom transformation. - fillTransform(odo_ros_msg.header.frame_id, odom_base_frame_id_, odo_ros_msg.pose.pose, transform); + fillTransform(odom_base_frame_id_, odo_ros_msg.header.frame_id, odo_ros_msg.pose.pose, transform); tf_broadcaster_->sendTransform(transform); }