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