def __init__(self):
#trajectory node
rospy.loginfo("Initializing trajectory node")
rospy.init_node('trajectory', anonymous=True)
self.reached = String("Halt")
rospy.loginfo("Initializing trajectory publishers")
self.waypointPub = rospy.Publisher("waypoint", String, queue_size=1)
self.waypoint_x = [0, 70, 80, 10, 0]
self.waypoint_y = [0, 0, -130, -130, -5]
if len(self.waypoint_x) == len(self.waypoint_y) & len(
self.waypoint_y) >= 2:
rospy.loginfo("Setting waypoints...")
else:
rospy.errinfo("Lengths of waypoint lists are not the same")
self.waypoint_Id = 0
self.waypoint = PoseStamped()
self.waypoint.header = "waypoints"
if __name__ == "__main__":
try:
# ROS Init
rospy.init_node('multi_goals', anonymous=True)
# Get params
goalListX = rospy.get_param('~goalListX', '[2.0, 2.0]')
goalListY = rospy.get_param('~goalListY', '[2.0, 4.0]')
map_frame = "map" # rospy.get_param('~map_frame', 'map' )
retry = 1 # rospy.get_param('~retry', '1')
goalListX = goalListX.replace("[", "").replace("]", "")
goalListY = goalListY.replace("[", "").replace("]", "")
goalListX = [float(x) for x in goalListX.split(",")]
goalListY = [float(y) for y in goalListY.split(",")]
print(goalListX)
print(goalListY)
goalListX = [2.69, 4.68, 3.6, 1.13, -0.83, -2.47, -5.49, -6.28, -4.24]
goalListY = [1.34, -1.5, -3.76, -4.48, -2.86, 0.76, 1.85, -1.19, -3.73]
if len(goalListX) == len(goalListY) & len(goalListY) >= 2:
# Constract MultiGoals Obj
rospy.loginfo("Multi Goals Executing...")
mg = MultiGoals(goalListX, goalListY, retry, map_frame)
rospy.spin()
else:
rospy.errinfo("Lengths of goal lists are not the same")
except KeyboardInterrupt:
print("shutting down")