mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-13 06:40:19 +08:00
61 lines
2.0 KiB
Python
Executable File
61 lines
2.0 KiB
Python
Executable File
#!/usr/bin/env python
|
|
import rospy
|
|
import sys
|
|
from std_msgs.msg import Bool
|
|
from rtabmap_ros.msg import Goal
|
|
|
|
pub = rospy.Publisher('/rtabmap/goal_node', Goal, queue_size=1)
|
|
waypoints = []
|
|
currentIndex = 0
|
|
|
|
def callback(data):
|
|
global currentIndex
|
|
if data.data:
|
|
rospy.loginfo(rospy.get_caller_id() + "Goal '%s' reached! Publishing next goal in 1 sec...", waypoints[currentIndex])
|
|
else:
|
|
rospy.loginfo(rospy.get_caller_id() + "Goal '%s' failed! Publishing next goal in 1 sec...", waypoints[currentIndex])
|
|
|
|
currentIndex = (currentIndex+1) % len(waypoints)
|
|
|
|
rospy.sleep(1.)
|
|
|
|
msg = Goal()
|
|
if waypoints[currentIndex].isdigit():
|
|
msg.node_id = int(waypoints[currentIndex])
|
|
msg.node_label = ""
|
|
else:
|
|
msg.node_id = 0
|
|
msg.node_label = waypoints[currentIndex]
|
|
msg.header.stamp = rospy.get_rostime()
|
|
rospy.loginfo("Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
|
|
pub.publish(msg)
|
|
|
|
def main():
|
|
rospy.init_node('patrol', anonymous=False)
|
|
rospy.Subscriber("/rtabmap/goal_reached", Bool, callback)
|
|
rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
|
|
|
|
# send the first goal
|
|
msg = Goal()
|
|
if waypoints[currentIndex].isdigit():
|
|
msg.node_id = int(waypoints[currentIndex])
|
|
msg.node_label = ""
|
|
else:
|
|
msg.node_id = 0
|
|
msg.node_label = waypoints[currentIndex]
|
|
while rospy.Time.now().secs == 0:
|
|
rospy.loginfo("Waiting clock...")
|
|
rospy.sleep(.1)
|
|
msg.header.stamp = rospy.Time.now()
|
|
rospy.loginfo("Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
|
|
pub.publish(msg)
|
|
rospy.spin()
|
|
|
|
if __name__ == '__main__':
|
|
if len(sys.argv) < 3:
|
|
print("usage: patrol.py waypointA waypointB waypointC ... (at least 2 waypoints, can be node id or label)")
|
|
else:
|
|
waypoints = sys.argv[1:]
|
|
rospy.loginfo("Waypoints: [%s]", str(waypoints).strip('[]'))
|
|
main()
|