i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57: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
scripts/patrol.py
+4 -2
View File
@@ -4,7 +4,7 @@ import sys
from std_msgs.msg import Bool from std_msgs.msg import Bool
from rtabmap_ros.msg import Goal from rtabmap_ros.msg import Goal
pub = rospy.Publisher('/rtabmap/goal_node', Goal, queue_size=1) pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1)
waypoints = [] waypoints = []
currentIndex = 0 currentIndex = 0
waitingTime = 1.0 waitingTime = 1.0
@@ -41,13 +41,15 @@ def callback(data):
def main(): def main():
rospy.init_node('patrol', anonymous=False) rospy.init_node('patrol', anonymous=False)
rospy.Subscriber("/rtabmap/goal_reached", Bool, callback) sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
global waitingTime global waitingTime
waitingTime = rospy.get_param('~time', waitingTime) waitingTime = rospy.get_param('~time', waitingTime)
rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]')) rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]'))
rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime) rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime)
rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name)
rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name)
# send the first goal # send the first goal
msg = Goal() msg = Goal()
src/CoreWrapper.cpp
+11 -7
View File
@@ -2033,15 +2033,19 @@ void CoreWrapper::process(
mbClient_->cancelGoal(); 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; if(goalReachedPub_.getNumSubscribers())
result.data = rtabmap_.getPathStatus() > 0; {
goalReachedPub_.publish(result); 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 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->estimateVertexCount(links.size() * 2);
manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST ); 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 poseIterFrom = poses.find(iter->second.from());
std::map<int, rtabmap::Transform>::iterator poseIterTo = poses.find(iter->second.to()); std::map<int, rtabmap::Transform>::iterator poseIterTo = poses.find(iter->second.to());