How to send waypoints to move_base using rospy?
I'm having trouble creating a node to send waypoints to move_base
one by one as each are achieved. These waypoints are provided by another node. The code I have so far is pasted below. It seems to send the goal but the robot does not move towards any of the goals.
Any ideas what might be wrong? I don't think I can be sure that the action server is always up and running, although there have been a few occasions where the 1st waypoint has been sent as a goal and achieved. I'm not sure why this worked and why it did not move on to the next waypoints extracted from the PoseArray
.
import rospy
from geometry_msgs.msg import PoseArray
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
class WaypointFollower(object):
def __init__(self):
self.waypoint_poses_sub = rospy.Subscriber("/waypoint_poses", PoseArray,
self.waypoint_poses_callback, queue_size=1)
self.waypoint_goal = MoveBaseGoal()
self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.move_base_client.wait_for_server()
def waypoint_poses_callback(self, waypoint_pose_array_msg):
for waypoint in range(0, len(waypoint_pose_array_msg.poses)):
print('Waypoint: ' + str(waypoint + 1))
self.waypoint_goal.target_pose.pose = waypoint_pose_array_msg.poses[waypoint]
self.waypoint_goal.target_pose.header.stamp = rospy.Time.now()
self.waypoint_goal.target_pose.header.frame_id = 'map'
self.move_base_client.send_goal(self.waypoint_goal)
wait = self.move_base_client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return self.move_base_client.get_result()
if __name__ == '__main__':
rospy.init_node('waypoint_follower', anonymous=True)
Result = WaypointFollower()
if Result == 1:
print('Waypoint_follower node successfully executed')
I'm not sure and might be wrong but shouldn't the return statement be outside of the
for
loop otherwise it will just end the callback after the first waypoint is reached.Also you should try separating the
move_base_client
which handles a single goal and your callback which handles the PoseArray. Your callback can have afor
loop where it calls themove_base_client
for each pose in the PoseArray.Thanks - moving the
return
was the right way to do it! I've created a separatego_to_waypoint
function as suggested and explored a bit further. I've found, using the code snippet below, that when the first goal is sent, the server goes down and the loop moves to the next goal straight away. I can see thatself.move_base_client.wait_for_result()
is not actually waiting for the robot to arrive at the goal waypoint so the for loop continues and fails to send further goals as it finds the server to be down. Any ideas?Does you node directly print
'Waypoint_follower node successfully executed'
after executing? If so you might have to add rospy.spin() inif __name__ == "__main__":
to prevent you node from stopping after execution if it doesn't receive the subscriber request.Also
Result = WaypointFollower()
may not be doing what you think it is doing. Here the value ofResult
will be an object of typeWaypointFollower
and not the result ofself.waypoint_poses_callback()
.try changing
to
No,
'Waypoint_follower node successfully executed'
is not printed after executing. Actually, I'd previously removedrospy.spin()
because I only really want to subscribe to the latestPoseArray
after they've been picked manually in RVIZ, i.e. I don't want to be trying to process every one "live" whilst a user is picking them graphically. I'd found that withoutrospy.spin()
I was able to do this and work through each individual pose in the array to create validmove_base_client
goals. The current problem seems to be that my code just doesn't wait for goals to be achieved before moving on despite usingself.move_base_client.wait_for_result()
.