i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Calling move_base::cancelGoal() when rtabmap planning fails or /rtabmap/cancel_goal is called

Browse Source
This commit is contained in:
matlabbe
2015-09-27 12:03:41 -04:00
parent 7deff58782
commit 4931866baf
2 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/config/base_local_planner_params.yaml
+1 -1
View File
@@ -12,7 +12,7 @@ TrajectoryPlannerROS:
max_vel_theta: 0.5 max_vel_theta: 0.5
min_vel_theta: -0.5 min_vel_theta: -0.5
min_in_place_vel_theta: 0.25 min_in_place_vel_theta: 0.25
holonomic_robot: false holonomic_robot: true
xy_goal_tolerance: 0.25 xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25
src/CoreWrapper.cpp
+8 -4
View File
@@ -1258,6 +1258,10 @@ void CoreWrapper::process(
else else
{ {
ROS_WARN("Planning: Plan failed!"); ROS_WARN("Planning: Plan failed!");
if(mbClient_.isServerConnected())
{
mbClient_.cancelGoal();
}
} }
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
@@ -1919,10 +1923,6 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
rtabmap_.clearPath(0); rtabmap_.clearPath(0);
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
if(mbClient_.isServerConnected())
{
mbClient_.cancelGoal();
}
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
std_msgs::Bool result; std_msgs::Bool result;
@@ -1930,6 +1930,10 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
goalReachedPub_.publish(result); goalReachedPub_.publish(result);
} }
} }
if(mbClient_.isServerConnected())
{
mbClient_.cancelGoal();
}
return true; return true;
} }