From c1e00fe76da235a322453b8a396291f44dc65083 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 23 Mar 2026 15:06:57 +0700 Subject: [PATCH] fix bug --- .../Packages/move_base/src/move_base.cpp | 26 ++++++------------- 1 file changed, 8 insertions(+), 18 deletions(-) diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 19b58c9..5073b7a 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -3144,15 +3144,6 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro } std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); - // robot_geometry_msgs::PoseStamped goal_pose, global_pose; - // goal_pose = goal_pose_msg; - - // goal_pose.header.stamp = robot::Time(); // latest available - // try - // { - // tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time()); - // tf3::doTransform(goal_pose, global_pose, transform); - // } robot_geometry_msgs::PoseStamped global_pose; tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); robot_geometry_msgs::PoseStamped goal_pose; @@ -3218,17 +3209,16 @@ robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData() { robot_nav_msgs::Path path; tc_->getGlobalPlan(path.poses); - if (!path.poses.empty()) + robot_nav_msgs::Path global_path; + global_path.header.stamp = robot::Time::now(); + global_path.header.frame_id = planner_costmap_robot_->getGlobalFrameID(); + for(auto &p : path.poses) { - path.header.stamp = path.poses[0].header.stamp; - path.header.frame_id = path.poses[0].header.frame_id; + robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p); + pose.header.stamp = robot::Time::now(); + global_path.poses.push_back(goalToGlobalFrame(pose)); } - else - { - path.header.stamp = robot::Time::now(); - path.header.frame_id = planner_costmap_robot_->getGlobalFrameID(); - } - global_data_.plan = robot_nav_2d_utils::pathToPath(path); + global_data_.plan = robot_nav_2d_utils::pathToPath(global_path); } ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true); convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);