def main():
rospy.init_node('patrol', anonymous=False)
sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
global waitingTime
global frameId
waitingTime = rospy.get_param('~time', waitingTime)
frameId = rospy.get_param('~frame_id', frameId)
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() + ": 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
msg = Goal()
msg.frame_id = frameId
try:
int(waypoints[currentIndex])
is_dig = True
except ValueError:
is_dig = False
if is_dig:
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(rospy.get_caller_id() + ": Waiting clock...")
rospy.sleep(.1)
msg.header.stamp = rospy.Time.now()
rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
pub.publish(msg)
rospy.spin()
def callback(data):
global currentIndex
global waitingTime
global frameId
if data.data:
rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
else:
rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
currentIndex = (currentIndex+1) % len(waypoints)
# Waiting time before sending next goal
rospy.sleep(waitingTime)
msg = Goal()
msg.frame_id = frameId
try:
int(waypoints[currentIndex])
is_dig = True
except ValueError:
is_dig = False
if is_dig:
msg.node_id = int(waypoints[currentIndex])
msg.node_label = ""
else:
msg.node_id = 0
msg.node_label = waypoints[currentIndex]
rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
msg.header.stamp = rospy.get_rostime()
pub.publish(msg)
def main():
rospy.init_node('patrol', anonymous=False)
sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
global waitingTime
global frameId
waitingTime = rospy.get_param('~time', waitingTime)
frameId = rospy.get_param('~frame_id', frameId)
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() + ": 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
msg = Goal()
msg.frame_id = frameId
try:
int(waypoints[currentIndex])
is_dig = True
except ValueError:
is_dig = False
if is_dig:
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(rospy.get_caller_id() + ": Waiting clock...")
rospy.sleep(.1)
msg.header.stamp = rospy.Time.now()
rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
pub.publish(msg)
rospy.spin()
def callback(data):
global currentIndex
global waitingTime
global frameId
if data.data:
rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
else:
rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
currentIndex = (currentIndex+1) % len(waypoints)
# Waiting time before sending next goal
rospy.sleep(waitingTime)
msg = Goal()
msg.frame_id = frameId
try:
int(waypoints[currentIndex])
is_dig = True
except ValueError:
is_dig = False
if is_dig:
msg.node_id = int(waypoints[currentIndex])
msg.node_label = ""
else:
msg.node_id = 0
msg.node_label = waypoints[currentIndex]
rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
msg.header.stamp = rospy.get_rostime()
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()
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)
