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

updated planning fail msg when the local map is broken

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-18 16:58:21 -05:00
parent 22f2246a05
commit 9c823cb93e
1 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+18 -2
View File
@@ -1019,7 +1019,15 @@ void CoreWrapper::process(
} }
else else
{ {
ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId()); ROS_ERROR("Planning: Local map broken (the robot may have moved to far from planned nodes)");
rtabmap_.clearPath();
if(goalReachedPub_.getNumSubscribers())
{
std_msgs::Bool result;
result.data = false;
goalReachedPub_.publish(result);
}
currentMetricGoal_.setNull();
} }
} }
} }
@@ -1105,7 +1113,9 @@ void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform>
} }
else else
{ {
ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).", rtabmap_.getGoalReachedRadius()); ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm) or too far from the graph (RGBD/GoalMaxDistance=%fm).",
rtabmap_.getGoalReachedRadius(),
rtabmap_.getGoalMaxDistance());
rtabmap_.clearPath(); rtabmap_.clearPath();
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
@@ -1125,7 +1135,9 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
return; return;
} }
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str()); ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
UTimer timer;
rtabmap_.computePath(targetPose, false); rtabmap_.computePath(targetPose, false);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath()); goalCommonCallback(rtabmap_.getPath());
} }
@@ -1138,7 +1150,9 @@ void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr &
return; return;
} }
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str()); ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
UTimer timer;
rtabmap_.computePath(targetPose, true); rtabmap_.computePath(targetPose, true);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath()); goalCommonCallback(rtabmap_.getPath());
} }
@@ -1478,7 +1492,9 @@ bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ro
{ {
int id = req.target_node_id; int id = req.target_node_id;
ROS_INFO("Planning: set goal %d", id); ROS_INFO("Planning: set goal %d", id);
UTimer timer;
rtabmap_.computePath(id, req.in_global_graph); rtabmap_.computePath(id, req.in_global_graph);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath()); goalCommonCallback(rtabmap_.getPath());
return !currentMetricGoal_.isNull(); return !currentMetricGoal_.isNull();
} }