text: #!/usr/bin/env python
  
  import numpy as np
  import rospy
  from geometry_msgs.msg import PoseStamped, TwistStamped
  from styx_msgs.msg import Lane, Waypoint
  import numpy as np
  from scipy.spatial import KDTree
  import math
  from std_msgs.msg import Int32, Bool
  
  '''
  This node will publish waypoints from the car's current position to some `x` distance ahead.
  
  As mentioned in the doc, you should ideally first implement a version which does not care
  about traffic lights or obstacles.
  
  Once you have created dbw_node, you will update this node to use the status of traffic lights too.
  
  Please note that our simulator also provides the exact location of traffic lights and their
  current status in `/vehicle/traffic_lights` message. You can use this message to build this node
  as well as to verify your TL classifier.
  
  TODO (for Yousuf and Aaron): Stopline location for each traffic light.
  '''
  
  LOOKAHEAD_WPS = 100 # Number of waypoints we will publish. You can change this number
  MAX_DECEL =  .5
  PUBLISH_RATE = 10
  
  class WaypointUpdater(object):
      def __init__(self):
          rospy.init_node('waypoint_updater')
  
          self.base_lane = None
          self.pose = None
          self.stopline_wp_idx = -1
          # self.base_waypoints = None
          self.waypoints_2d = None
          self.waypoint_tree = None
          self.tl_detector_initialized = False
          rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
          self.base_waypoints_sub = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
          rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb)
          rospy.Subscriber('/tl_detector_initialized',Bool,self.tl_detector_initialized_cb)
  
          self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
  
          self.loop()
          # rospy.spin()
      def loop(self):
          rate = rospy.Rate(PUBLISH_RATE)
          while not rospy.is_shutdown():
              if not self.tl_detector_initialized:
                  rospy.logwarn('Waypoint Updater waiting for TL detector to be initialized')
                  rate.sleep()
                  continue
  
              if self.pose and self.waypoint_tree:
                  self.publish_waypoints()
              rate.sleep()
  
      def get_closest_waypoint_idx(self):
          x = self.pose.pose.position.x
          y = self.pose.pose.position.y
          closest_idx = self.waypoint_tree.query([x,y],1)[1]
  
          #check if closes is ahead or behind vehicle
          closet_coord = self.waypoints_2d[closest_idx]
          prev_coord = self.waypoints_2d[closest_idx-1]
  
          #Equation for hyperlane though closest_coords
          cl_vect = np.array(closet_coord)
          prev_vect = np.array(prev_coord)
          pos_vect = np.array([x,y])
  
          val = np.dot(cl_vect-prev_vect,pos_vect-cl_vect)
  
          if val > 0:
              closest_idx = (closest_idx + 1) % len(self.waypoints_2d)
          return closest_idx
  
      def publish_waypoints(self):
          final_lane = self.generate_lane()
          self.final_waypoints_pub.publish(final_lane)
  
      def generate_lane(self):
          lane = Lane()
          lane.header = self.base_lane.header
          closest_idx = self.get_closest_waypoint_idx()
          farthest_idx = closest_idx + LOOKAHEAD_WPS
          base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx]
  
          if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx):
              lane.waypoints = base_waypoints
          else:
              lane.waypoints = self.decelerate_waypoints(base_waypoints,closest_idx)
          rospy.loginfo("Closest Stop Line: [index=%d]",self.stopline_wp_idx)
          return lane
  
      def decelerate_waypoints(self, waypoints, closest_idx):
          temp = []
          for i, wp in enumerate(waypoints):
              p = Waypoint()
              p.pose = wp.pose
              stop_idx = max(self.stopline_wp_idx-closest_idx-5,0) # Stop line margin 2 to 4
              dist = self.distance(waypoints,i,stop_idx)
              vel = math.sqrt( 2 * MAX_DECEL * dist) #  math.sqrt( 2 * MAX_DECEL * dist)
              if ((vel < 1.) or((self.stopline_wp_idx>=i)and(i>=stop_idx))):
                  vel = 0.0
              p.twist.twist.linear.x = min(vel,wp.twist.twist.linear.x)
              temp.append(p)
          return temp
  
      def pose_cb(self, msg):
          self.pose = msg
  
      def waypoints_cb(self, waypoints):
          self.base_lane = waypoints
  
          if not self.waypoints_2d:
              self.waypoints_2d = [[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] for waypoint in self.base_lane.waypoints]
              self.waypoint_tree = KDTree(self.waypoints_2d)
  
          # Unsubscribe as base waypoints are not needed
          self.base_waypoints_sub.unregister()
  
      def traffic_cb(self, msg):
          #  Callback for /traffic_waypoint message.
          self.stopline_wp_idx = msg.data
  
      def tl_detector_initialized_cb(self,msg):
          self.tl_detector_initialized = True
  
      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
  
  
  if __name__ == '__main__':
      try:
          WaypointUpdater()
      except rospy.ROSInterruptException:
          rospy.logerr('Could not start waypoint updater node.')
  