How to send waypoints to move_base using rospy?

asked 2021-03-08 11:15:28 -0500

Py gravatar image

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')
edit retag flag offensive close merge delete

Comments

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 a for loop where it calls the move_base_client for each pose in the PoseArray.

def go_to_waypoint(Pose):
    # call move_base_client here i.e send goal

def waypoint_poses_callback(PoseArray):
   for (Pose in PoseArray):
        # call go_to_waypoint(Pose) here
shlock gravatar image shlock  ( 2021-03-08 22:57:32 -0500 )edit

Thanks - moving the return was the right way to do it! I've created a separate go_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 that self.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?

def go_to_waypoint(self, goal):
    server_up = self.move_base_client.wait_for_server()
    if server_up:
        self.move_base_client.send_goal(goal)
        self.move_base_client.wait_for_result()
        server_check = self.move_base_client.wait_for_server(timeout=rospy.Duration(10.0))
        print('Server up after goal send?: ' + str(server_check))
    else:
        print("Cannot send goal to move_base. Server down")
Py gravatar image Py  ( 2021-03-09 03:44:23 -0500 )edit
1

Does you node directly print 'Waypoint_follower node successfully executed' after executing? If so you might have to add rospy.spin() in if __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 of Result will be an object of type WaypointFollower and not the result of self.waypoint_poses_callback().

try changing

if __name__ == '__main__':
    rospy.init_node('waypoint_follower', anonymous=True)
    Result = WaypointFollower()

to

if __name__ == '__main__':
    rospy.init_node('waypoint_follower', anonymous=True)
    Result = WaypointFollower()
    rospy.spin()
shlock gravatar image shlock  ( 2021-03-09 04:11:01 -0500 )edit

No, 'Waypoint_follower node successfully executed' is not printed after executing. Actually, I'd previously removed rospy.spin() because I only really want to subscribe to the latest PoseArray 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 without rospy.spin() I was able to do this and work through each individual pose in the array to create valid move_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 using self.move_base_client.wait_for_result().

Py gravatar image Py  ( 2021-03-09 04:53:56 -0500 )edit