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 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()
