def purepursuit_client(): # Creates the SimpleActionClient, passing the type of the action # (GoTo) to the constructor. client = actionlib.SimpleActionClient('purepursuit_planner', GoToAction) # Waits until the action server has started up and started listening for goals. client.wait_for_server() g = GoToGoal(target=[ goal(pose=Pose2D(-2.0, 0.86, 0.0), speed=0.2), goal(pose=Pose2D(-1.0, 0.46, 0.0), speed=0.2), goal(pose=Pose2D(0.0, 0.0, 0.0), speed=0.2), goal(pose=Pose2D(1.0, 0.0, 0.0), speed=0.2), goal(pose=Pose2D(2.54, 1.0, 0.0), speed=0.2), goal(pose=Pose2D(4.96, 2.18, 0.0), speed=0.2), goal(pose=Pose2D(8.54, 2.22, 0.0), speed=0.2) ]) rospy.loginfo('Sending goal') # Sends the goal to the action server. client.send_goal(g) #print 'Sleeping' #time.sleep(4) #print 'Canceling goal' #client.cancel_goal() # Waits for the server to finish performing the action. client.wait_for_result() # Prints out the result of executing the action return client.get_result()
def goTo(self, goal_list): # Waits until the action server has started up and started # listening for goals. if self.client.wait_for_server(timeout = rospy.Duration(3.0) ): #if self.getState() != GoalStatus.LOST: # rospy.loginfo('PurepursuitClient: planner is tracking a goal') # return -2 g = GoToGoal(target = goal_list) rospy.loginfo('PurepursuitClient: Sendig %d waypoints'%(len(goal_list))) self.client.send_goal(g) return 0 else: rospy.logerr('PurepursuitClient: Error waiting for server') return -1