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

Planning: if use_action_for_goal is true, wait for move_base status before sending goal status. Updated patrol.py

Browse Source
This commit is contained in:
matlabbe
2019-01-15 18:12:11 -05:00
parent 19433f809f
commit ba5738354d
3 changed files with 16 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+11 -7
View File
@@ -2033,15 +2033,19 @@ void CoreWrapper::process(
mbClient_->cancelGoal();
}
}
if(goalReachedPub_.getNumSubscribers())
// Don't send status yet, let move_base finish reaching the goal
if(mbClient_ == 0 || rtabmap_.getPathStatus() <= 0)
{
std_msgs::Bool result;
result.data = rtabmap_.getPathStatus() > 0;
goalReachedPub_.publish(result);
if(goalReachedPub_.getNumSubscribers())
{
std_msgs::Bool result;
result.data = rtabmap_.getPathStatus() > 0;
goalReachedPub_.publish(result);
}
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
}
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
}
else
{
src/rviz/MapGraphDisplay.cpp
+1 -1
View File
@@ -131,7 +131,7 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg
manual_object->estimateVertexCount(links.size() * 2);
manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
std::map<int, rtabmap::Transform>::iterator poseIterFrom = poses.find(iter->second.from());
std::map<int, rtabmap::Transform>::iterator poseIterTo = poses.find(iter->second.to());