Skip to content

Commit

Permalink
BUG FIX: Published odometry msg lacked target frame_id
Browse files Browse the repository at this point in the history
  • Loading branch information
jlblancoc committed Dec 20, 2024
1 parent cbea314 commit d58a0a2
Showing 1 changed file with 2 additions and 0 deletions.
2 changes: 2 additions & 0 deletions mola_bridge_ros2/src/BridgeROS2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -799,6 +799,7 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationRobotPose& obs)
nav_msgs::msg::Odometry msg;
msg.header.stamp = myNow(obs.timestamp);
msg.header.frame_id = params_.reference_frame;
msg.child_frame_id = params_.base_link_frame;

msg.pose = mrpt::ros2bridge::toROS_Pose(obs.pose);

Expand Down Expand Up @@ -1316,6 +1317,7 @@ void BridgeROS2::timerPubLocalization()
// Convert observation MRPT -> ROS
nav_msgs::msg::Odometry msg;
msg.header.stamp = myNow(l->timestamp);
msg.child_frame_id = params_.base_link_frame;
msg.header.frame_id = params_.reference_frame;

mrpt::poses::CPose3DPDFGaussian posePdf;
Expand Down

0 comments on commit d58a0a2

Please sign in to comment.