Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Transforms to global frame before checking quaternion #1261

Open
wants to merge 1 commit into
base: noetic-devel
Choose a base branch
from
Open

Transforms to global frame before checking quaternion #1261

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions move_base/src/move_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -650,13 +650,13 @@ namespace move_base {

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
{
if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

if(!isQuaternionValid(goal.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}

geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

publishZeroVelocity();
//we have a goal so start the planner
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
Expand Down Expand Up @@ -693,14 +693,14 @@ namespace move_base {
if(as_->isPreemptRequested()){
if(as_->isNewGoalAvailable()){
//if we're active and a new goal is available, we'll accept it, but we won't shut anything down
move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
move_base_msgs::MoveBaseGoal new_move_base_goal = *as_->acceptNewGoal();
geometry_msgs::PoseStamped new_goal = goalToGlobalFrame(new_move_base_goal.target_pose);

if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
if(!isQuaternionValid(new_goal.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}

goal = goalToGlobalFrame(new_goal.target_pose);
goal = new_goal;

//we'll make sure that we reset our state for the next execution cycle
recovery_index_ = 0;
Expand Down