class WaypointUpdater(object):
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb)
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_lights_cb)
self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
self.speed_limit = rospy.get_param('/waypoint_loader/velocity') * 1000 / 3600.
# TODO: Add other member variables you need below
self.light_wp = None
self.lights = None
self.current_pose = None
self.base_waypoints = None
self.planner = PathPlanner(LOOKAHEAD_WPS)
self.planner.set_speed_limit(self.speed_limit)
# rospy.spin()
def publish(self, waypoints):
if waypoints == []:
return
lane = Lane()
lane.header.frame_id = '/world'
lane.header.stamp = rospy.get_rostime()
lane.waypoints = waypoints
self.final_waypoints_pub.publish(lane)
def run(self):
rospy.loginfo('### Run')
rate = rospy.Rate(5) # 10hz
while not rospy.is_shutdown():
# rospy.loginfo('### looping ... ')
waypoints = self.planner.generate_waypoints()
self.publish(waypoints)
rate.sleep()
def pose_cb(self, msg):
# msg - geometry_msgs/PoseStamped
# rospy.loginfo('### pose Received')
self.current_pose = msg
self.planner.update_vehicle_location(self.current_pose)
def waypoints_cb(self, lane):
# waypoints - styx_msgs/Lane, only received once
rospy.loginfo('### BASE Waypoint Received. speed_limit: %f', self.speed_limit)
self.base_waypoints = lane.waypoints
self.planner.set_base_waypoints(lane.waypoints)
def traffic_cb(self, msg):
# msg - std_msgs/Int32
self.light_wp = msg.data
self.planner.update_tl_wp(self.light_wp)
rospy.loginfo('### TrafficLightIndex Received: %i', msg.data)
def traffic_lights_cb(self, msg):
# msg - styx_msgs/TrafficLightArray
self.lights = msg.lights
# self.planner.update_traffic_lights(msg.lights)
def obstacle_cb(self, msg):
# TODO: Callback for /obstacle_waypoint message. We will implement it later
pass
def distance(self, waypoints, wp1, wp2):
dist = 0
dl = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2)
for i in range(wp1, wp2+1):
dist += dl(waypoints[wp1].pose.pose.position, waypoints[i].pose.pose.position)
wp1 = i
return dist
class WaypointUpdater(object):
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
# TODO: Add other member variables you need below
self.current_pose = None
self.base_waypoints = None
self.planner = PathPlanner(LOOKAHEAD_WPS)
# rospy.spin()
def publish(self, waypoints):
if waypoints == []:
return
lane = Lane()
lane.header.frame_id = '/world'
lane.header.stamp = rospy.get_rostime()
lane.waypoints = waypoints
self.final_waypoints_pub.publish(lane)
def run(self):
rospy.loginfo('### Run')
rate = rospy.Rate(2) # 10hz
while not rospy.is_shutdown():
rospy.loginfo('### looping ... ')
waypoints = self.planner.generate_waypoints()
self.publish(waypoints)
rate.sleep()
def pose_cb(self, msg):
# msg - geometry_msgs/PoseStamped
# rospy.loginfo('### pose Received')
self.current_pose = msg
self.planner.update_vehicle_location(self.current_pose)
def waypoints_cb(self, lane):
# waypoints - styx_msgs/Lane, only received once
rospy.loginfo('### BASE Waypoint Received')
self.base_waypoints = lane.waypoints
self.planner.set_base_waypoints(lane.waypoints)
def traffic_cb(self, msg):
# TODO: Callback for /traffic_waypoint message. Implement
pass
def obstacle_cb(self, msg):
# TODO: Callback for /obstacle_waypoint message. We will implement it later
pass
def get_waypoint_velocity(self, waypoint):
return waypoint.twist.twist.linear.x
def set_waypoint_velocity(self, waypoints, waypoint, velocity):
waypoints[waypoint].twist.twist.linear.x = velocity
def distance(self, waypoints, wp1, wp2):
dist = 0
dl = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2)
for i in range(wp1, wp2+1):
dist += dl(waypoints[wp1].pose.pose.position, waypoints[i].pose.pose.position)
wp1 = i
return dist
