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

update rotate to goal

Browse Source
This commit is contained in:
HiepLM
2026-02-05 16:58:55 +07:00
parent bf74ae84ba
commit 6e320bbe5c
7 changed files with 48 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1139,10 +1139,26 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
try
{
robot_geometry_msgs::Pose2D pose;
if (!this->getRobotPose(pose))
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
nav_feedback_->goal_checked = false;
}
return false;
}
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
double distance = planner_costmap_robot_ ? planner_costmap_robot_->getCostmap()->getResolution() : 0.05;
action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta);
action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta);
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;