HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix bug

Browse Source
This commit is contained in:
HiepLM
2026-03-23 15:06:57 +07:00
parent 472cc4d02c
commit c1e00fe76d
1 changed files with 8 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -3144,15 +3144,6 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro
} }
std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); 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; robot_geometry_msgs::PoseStamped global_pose;
tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
robot_geometry_msgs::PoseStamped goal_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; robot_nav_msgs::Path path;
tc_->getGlobalPlan(path.poses); 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; robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p);
path.header.frame_id = path.poses[0].header.frame_id; pose.header.stamp = robot::Time::now();
global_path.poses.push_back(goalToGlobalFrame(pose));
} }
else global_data_.plan = robot_nav_2d_utils::pathToPath(global_path);
{
path.header.stamp = robot::Time::now();
path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
}
global_data_.plan = robot_nav_2d_utils::pathToPath(path);
} }
ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true); 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); convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);