I prepared an example for you.
In follow_waypoints.py
find class FollowPath(State)
and modify its method execute(self, userdata)
:
def execute(self, userdata):
global waypoints
# Execute waypoints each in sequence
for waypoint in waypoints:
# Break if preempted
if waypoints == []:
rospy.loginfo('The waypoint queue has been reset.')
break
trigger_topic = "/follow_trigger"
self.wait_for_trigger = True
while self.wait_for_trigger:
try:
trigger_msg = rospy.wait_for_message(trigger_topic, Bool, timeout=1)
except rospy.ROSException as e:
if 'timeout exceeded' in e.message:
continue
else:
raise e
rospy.loginfo("Proceed to next waypoint")
self.wait_for_trigger = False
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = self.frame_id
goal.target_pose.pose.position = waypoint.pose.pose.position
goal.target_pose.pose.orientation = waypoint.pose.pose.orientation
rospy.loginfo('Executing move_base goal to position (x,y): %s, %s' %
(waypoint.pose.pose.position.x, waypoint.pose.pose.position.y))
rospy.loginfo("To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'")
self.client.send_goal(goal)
self.client.wait_for_result()
return 'success'
This way robot will wait for any message on /follow_trigger
topic when navigating to each point.
To publish a message:
rostopic pub /follow_trigger std_msgs/Bool true
Regards,
Łukasz