def convertListOfPointPathIntoGoal(self, inverse = False): converted_list = [] if inverse: for i in reversed(self.list_of_points): converted_list.append(goal(pose = Pose2D(i.pose.position.x, i.pose.position.y, 0.0), speed = i.speed)) # For now speed constant else: for i in self.list_of_points: #print 'convertListOfPointPathIntoGoal: %.2lf, %.2lf'%(i.pose.position.x, i.pose.position.y) converted_list.append(goal(pose = Pose2D(i.pose.position.x, i.pose.position.y, 0.0), speed = i.speed)) # For now speed constant return converted_list
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 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()