def decelerate_waypoints(self, waypoints, closest_idx): result = [] # How many waypoints ahead we want to stop. stop_idx = max(self.stopline_wp_idx - closest_idx - 3, 0) SCALING_FACTOR = 10 # change the deceleration profile - higher values will make the car decelerate only when closer to the traffic light dist = self.distance(waypoints, 0, stop_idx) max_allowed_vel = math.sqrt(SCALING_FACTOR * MAX_DECEL * dist) # rospy.logwarn( # "max_vel_to_stop: {}, curr_vel: {}".format( # max_allowed_vel, self.current_vel # ) # ) if max_allowed_vel > self.current_vel: return waypoints for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose p.twist = copy.deepcopy(wp.twist) dist = self.distance(waypoints, i, stop_idx) vel = math.sqrt(SCALING_FACTOR * MAX_DECEL * dist) vel = max(0.0, vel) p.twist.twist.linear.x = min(vel, p.twist.twist.linear.x) result.append(p) return result
def publish(self): if self.waypoints is None or self.pose is None: return num_waypoints_in_list = len(self.waypoints.waypoints) # Gererate an empty lane to store the final_waypoints lane = Lane() lane.header.frame_id = self.waypoints.header.frame_id lane.header.stamp = rospy.Time(0) lane.waypoints = [] # Iterate through the complete set of waypoints until we found the closest #rospy.loginfo('Started at waypoint index: %s', self.prev_first_wpt_index) #start_time = time.time() first_wpt_index = -1 min_wpt_distance = float('inf') for index, waypoint in enumerate(self.waypoints.waypoints): current_wpt_distance = self.distance(self.pose.pose.position, waypoint.pose.pose.position) if current_wpt_distance > 0 and current_wpt_distance < min_wpt_distance: min_wpt_distance = current_wpt_distance first_wpt_index = index self.waypoints.waypoints[first_wpt_index].pose.header.frame_id = self.waypoints.header.frame_id try: self.tf_listener.waitForTransform("base_link", "world", rospy.Time(0), rospy.Duration(0.05)) transformed_waypoint = self.tf_listener.transformPose("base_link", self.waypoints.waypoints[first_wpt_index].pose) except (tf.Exception, tf.LookupException, tf.ConnectivityException): rospy.logwarn("fail %s", time.time()) return if transformed_waypoint.pose.position.x <= 0.0: first_wpt_index += 1 first_wpt_index %= num_waypoints_in_list planned_velocity = 4.4 for num_wp in range(LOOKAHEAD_WPS): wp = Waypoint() wp.pose = self.waypoints.waypoints[(first_wpt_index + num_wp) % num_waypoints_in_list].pose wp.twist = self.waypoints.waypoints[(first_wpt_index + num_wp) % num_waypoints_in_list].twist if self.light_waypoint_index > -1: planned_velocity -= 0.1 if planned_velocity < 0.1: planned_velocity = 0 wp.twist.twist.linear.x = planned_velocity wp.twist.twist.linear.y = 0.0 wp.twist.twist.linear.z = 0.0 wp.twist.twist.angular.x = 0.0 wp.twist.twist.angular.y = 0.0 wp.twist.twist.angular.z = 0.0 lane.waypoints.append(wp) # finally, publish waypoints as modified on /final_waypoints topic self.final_waypoints_pub.publish(lane)
def publish(self): # Define container for waypoints ahead wps_ahead = Lane() # Determine length of input waypoints waypoints_len = len(self.base_waypoints) # Determine first waypoint ahead of the car # Searching for closest waypoint (Just ID) idx_wp_closest = self.get_idx_closest_waypoint() # Seeing if closest waypoint is ahead, otherwise choose the next (Just ID) idx_wp_ahead = self.get_idx_ahead_waypoint(idx_wp_closest) # Setting last_waypoint_id now self.last_waypoint_id = idx_wp_ahead # Determine waypoints ahead to be published idx_cur = idx_wp_ahead # Going through the whole list of traffic points we want to publish for i in range(LOOKAHEAD_WPS): # Setting the pose and twist data for the waypoints in the list wp = self.base_waypoints[idx_cur] next_wp = Waypoint() next_wp.pose = wp.pose next_wp.twist = wp.twist # Resetting our speed for the waypoints, if the car is getting from stopping to driving again if (self.traffic_light_helper > 0) & (self.drive > 0): next_wp.twist.twist.linear.x = self.get_waypoint_velocity( self.base_waypoints_copy[idx_cur]) # Append the waypoint to wps_ahead wps_ahead.waypoints.append(next_wp) # For the wrap-around last waypoint to first waypoint idx_cur = (idx_cur + 1) % waypoints_len # Setting drive-parameter when we want to driving again if (self.traffic_light_helper > 0) & (self.drive > 0): self.drive = 0 # Stopping if a traffic_light is red in front of us if self.tf_state > -1: self.stopping(self.tf_state, idx_wp_ahead, wps_ahead) # Setting wps_ahead to self.wps_ahead self.wps_ahead = wps_ahead # Publish our waypoints! self.final_waypoints_pub.publish(wps_ahead)
def publish(self): wps_pub = Lane() self.find_next_waypoint() #determine the list of way points to publish if self.next_waypoint_index is not None: wp_index = self.next_waypoint_index for i in range(LOOKAHEAD_WPS): start_wp = self.base_waypoints[wp_index] next_wp = Waypoint() next_wp.pose = start_wp.pose next_wp.twist = start_wp.twist wps_pub.waypoints.append(next_wp) wp_index = (wp_index + 1) % self.num_waypoints #rospy.logwarn('way points published:{}'.format(len(wps_pub.waypoints))) self.final_waypoints_pub.publish(wps_pub)
def get_final_waypoints(self, waypoints, start_wp, end_wp): final_waypoints = [] if end_wp < start_wp: # deal with the corner case that next traffic way point crossed the end of the track end_wp += len(waypoints) for i in range(start_wp, end_wp + 1): wp = Waypoint() index = i % len(waypoints) wp.pose = copy.deepcopy(waypoints[index].pose) wp.twist = copy.deepcopy(waypoints[index].twist) final_waypoints.append(wp) if self.braking: final_waypoints = self.decelerate(final_waypoints) else: final_waypoints = self.accelerate(final_waypoints) return final_waypoints
def publish(self): wps_pub = Lane() self.find_next_waypoint() #determine the list of way points to publish distance_tl = None #check red light distance #TODO will it detect yellow light? if self.red_light_index and self.next_waypoint_index: #distance to the next traffic light distance_tl = self.distance(self.base_waypoints, self.next_waypoint_index, self.red_light_index) distance_tl = distance_tl - DIST_LIGHT_LINE #minus the distance from stop line to the traffic light rospy.logwarn("Car is {:.2f} meters from the red light".format(distance_tl)) #check car's current speed if self.twist: self.current_velocity = self.twist.twist.linear.x #meters per second self.current_velocity_mph = 2.23694 * self.current_velocity # miles per hour #rospy.logwarn("Current speed is {:.2f} MPH".format(self.current_velocity_mph)) #Stop the car if it cannot pass the line within 2 seconds if distance_tl and distance_tl / self.current_velocity > 2: set_speed = 0 else: set_speed = self.max_velocity_km if self.next_waypoint_index is not None: wp_index = self.next_waypoint_index for i in range(LOOKAHEAD_WPS): start_wp = self.base_waypoints[wp_index] next_wp = Waypoint() next_wp.pose = start_wp.pose next_wp.twist = start_wp.twist next_wp.twist.twist.linear.x = set_speed wps_pub.waypoints.append(next_wp) wp_index = (wp_index+1) % self.num_waypoints #rospy.logwarn('way points published:{}'.format(len(wps_pub.waypoints))) self.final_waypoints_pub.publish(wps_pub)
def publish(self): # Define container for waypoints ahead wps_ahead = Lane() # Determine length of input waypoints waypoints_len = len(self.base_waypoints) # Determine first waypoint ahead of the car idx_wp_closest = self.get_idx_closest_waypoint() idx_wp_ahead = self.get_idx_ahead_waypoint(idx_wp_closest) self.last_waypoint_id = idx_wp_ahead # Determine waypoints ahead to be published idx_cur = idx_wp_ahead #rospy.loginfo(idx_cur) for i in range(LOOKAHEAD_WPS): wp = self.base_waypoints[idx_cur] next_wp = Waypoint() next_wp.pose = wp.pose next_wp.twist = wp.twist #if (self.need > 200) & (self.drive > 0): if (self.need > 0) & (self.drive > 0): next_wp.twist.twist.linear.x = self.save_velocity wps_ahead.waypoints.append(next_wp) idx_cur = (idx_cur + 1) % waypoints_len #if (self.need > 200) & (self.drive > 0): if (self.need > 0) & (self.drive > 0): self.drive = 0 #self.stopping(400, idx_wp_ahead, wps_ahead) #self.stopping(753, idx_wp_ahead, wps_ahead) #rospy.loginfo(self.tf_state) #rospy.loginfo(self.need) if self.tf_state > 300: #rospy.loginfo(self.tf_state) self.stopping(self.tf_state, idx_wp_ahead, wps_ahead) #rospy.loginfo(self.need) # Publish waypoints ahead self.wps_ahead = wps_ahead self.final_waypoints_pub.publish(wps_ahead)
def find_closest_waypoint_ahead_and_publish(self, waypoints): first_wpt_index = -1 min_wpt_dist = float('inf') dist_decreased = False local_coord_x = None num_waypoints = len(waypoints.waypoints) for index, waypoint in enumerate( waypoints.waypoints[self.former_first_wpt_index - 1:] + waypoints.waypoints[:self.former_first_wpt_index - 1], start=self.former_first_wpt_index - 1): try: tmp_wpt = waypoint tmp_wpt.pose.header.frame_id = waypoints.header.frame_id self.tf_listener.waitForTransform( '/base_link', '/world', rospy.Time(0), rospy.Duration(TIMEOUT_VALUE)) transformed_waypoint = self.tf_listener.transformPose( '/base_link', tmp_wpt.pose) current_wpt_dist = transformed_waypoint.pose.position.x**2 + transformed_waypoint.pose.position.y**2 if dist_decreased and current_wpt_dist > min_wpt_dist: # we have already gone past the closest waypoint rospy.loginfo( 'WaypointUpdater::waypoints_cb, found waypoint index %s', first_wpt_index) break if current_wpt_dist < min_wpt_dist: min_wpt_dist = current_wpt_dist first_wpt_index = index local_x_coord = transformed_waypoint.pose.position.x dist_decreased = True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr( 'WaypointUpdater::waypoints_cb - could not get coordinate frame transform!!!' ) first_wpt_index %= num_waypoints if first_wpt_index == -1: rospy.logwarn( 'WaypointUpdater::waypoints_cb - No waypoints ahead of ego were found... seems that the car went off course' ) else: if local_coord_x <= 0.0: first_wpt_index += 1 self.former_first_wpt_index = first_wpt_index % num_waypoints lane = Lane() lane.header.frame_id = waypoints.header.frame_id lane.header.stamp = rospy.Time(0) lane.waypoints = [] for num_wp in range(LOOKAHEAD_WPS): wp = Waypoint() wp.pose = waypoints.waypoints[(first_wpt_index + num_wp) % num_waypoints].pose wp.twist = waypoints.waypoints[(first_wpt_index + num_wp) % num_waypoints].twist wp.twist.twist.linear.x = self.default_velocity wp.twist.twist.linear.y = 0.0 wp.twist.twist.linear.z = 0.0 wp.twist.twist.angular.x = 0.0 wp.twist.twist.angular.y = 0.0 wp.twist.twist.angular.z = 0.0 lane.waypoints.append(wp) self.final_waypoints_pub.publish(lane) rospy.loginfo( 'waypoint_updater - publishing /final_waypoints with %s waypoints and first waypoint with index %s', len(lane.waypoints), first_wpt_index)
def publish(self): wps_pub = Lane() self.find_next_waypoint() #determine the list of way points to publish distance_tl = None state = "run" #check red light distance #TODO will it detect yellow light? rospy.logwarn("Index: waypoint, redlight {}, {}".format(self.next_waypoint_index,self.red_light_index)) if self.red_light_index and self.next_waypoint_index and self.red_light_index > self.next_waypoint_index: #distance to the next traffic light in meters distance_tl = self.distance(self.base_waypoints, self.next_waypoint_index, self.red_light_index) distance_tl = distance_tl - DIST_LIGHT_LINE #minus the distance from stop line to the traffic light rospy.logwarn("Car is {:.2f} meters from the stopping light".format(distance_tl)) #check car's current speed if self.twist: self.current_velocity = self.twist.twist.linear.x #meters per second self.current_velocity_mph = 2.23694 * self.current_velocity # miles per hour #rospy.logwarn("Current speed is {:.2f} MPH".format(self.current_velocity_mph)) #Stop the car if it cannot pass the line within 1.5 seconds. safe to brake if distance_tl and self.current_velocity > 0 and distance_tl / self.current_velocity > 1 and distance_tl < BRAKE_DIS: rospy.logwarn("Time to pass the stop line: {}".format(distance_tl / self.current_velocity)) state = "brake" else: #if it's green light ahead, or safe to pass through the light state = "run" max_speed = self.max_velocity_km*0.2778*0.9 #max speed in meter per second rospy.logwarn("Cat state is set to be {}".format(state)) if state == 'brake': index_stop_line = self.red_light_index - WP_GAP_LINE_LIGHT num_wp_stopping = index_stop_line - self.next_waypoint_index #number of ways points used to stop the car #linearly decelerate the car des = self.current_velocity / (num_wp_stopping +0.001) if self.next_waypoint_index is not None: wp_index = self.next_waypoint_index for i in range(LOOKAHEAD_WPS): base_wp = self.base_waypoints[wp_index] next_wp = Waypoint() next_wp.pose = base_wp.pose #position next_wp.twist = base_wp.twist #Speed #if the car shall brake, and setting reduced speed for all points leading to the stop line. if state == 'brake' and i <= num_wp_stopping: if i == num_wp_stopping: next_wp.twist.twist.linear.x = 0 # the way point that the car shall stop else: next_wp.twist.twist.linear.x = (self.current_velocity - des * (i+1)) else: next_wp.twist.twist.linear.x = max_speed#convert miles per hour to meters per second wps_pub.waypoints.append(next_wp) wp_index = (wp_index+1) % self.num_waypoints #rospy.logwarn('Next way point published:{}'.format(wps_pub)) #rospy.logwarn('way points published:{}'.format(len(wps_pub.waypoints))) self.final_waypoints_pub.publish(wps_pub)
def pose_cb(self, msg): self.pose = msg first_wpt_index = -1 min_wpt_distance = float('inf') if self.waypoints is None: return num_waypoints_in_list = len(self.waypoints.waypoints) # Generate an empty lane to store the final_waypoints lane = Lane() lane.header.frame_id = self.waypoints.header.frame_id lane.header.stamp = rospy.Time(0) lane.waypoints = [] # Iterate through the complete set of waypoints until we found the closest distance_decreased = False # rospy.loginfo('Started at waypoint index: %s', self.prev_first_wpt_index) # start_time = time.time() for index, waypoint in enumerate( self.waypoints.waypoints[self.prev_first_wpt_index:] + self.waypoints.waypoints[:self.prev_first_wpt_index], start=self.prev_first_wpt_index): current_wpt_distance = self.distance(self.pose.pose.position, waypoint.pose.pose.position) if distance_decreased and current_wpt_distance > min_wpt_distance: break if current_wpt_distance > 0 and current_wpt_distance < min_wpt_distance: min_wpt_distance = current_wpt_distance first_wpt_index = index distance_decreased = True first_wpt_index %= num_waypoints_in_list transformed_light_point = None # rospy.loginfo_throttle(1, 'Current waypoint: ' + str(self.prev_first_wpt_index) + ' of ' + str(len(self.waypoints.waypoints)) + ' ' + str(self.pose.pose.position.x)) if first_wpt_index == -1: rospy.logwarn( 'WaypointUpdater::waypoints_cb - No waypoints ahead of ego were found... seems that the car went off course' ) else: # transform fast avoiding wait cycles # Transform first waypoint to car coordinates self.waypoints.waypoints[ first_wpt_index].pose.header.frame_id = self.waypoints.header.frame_id try: self.tf_listener.waitForTransform("base_link", "world", rospy.Time(0), rospy.Duration(0.02)) transformed_waypoint = self.tf_listener.transformPose( "base_link", self.waypoints.waypoints[first_wpt_index].pose) except (tf.Exception, tf.LookupException, tf.ConnectivityException): try: self.tf_listener.waitForTransform( "base_link", "world", rospy.Time(0), rospy.Duration(TIMEOUT_VALUE)) transformed_waypoint = self.tf_listener.transformPose( "base_link", self.waypoints.waypoints[first_wpt_index].pose) except (tf.Exception, tf.LookupException, tf.ConnectivityException): rospy.logwarn("Failed to find camera to map transform") return # All waypoints in front of the car should have positive X coordinate in car coordinate frame # If the closest waypoint is behind the car, skip this waypoint if transformed_waypoint.pose.position.x <= 0.0: first_wpt_index += 1 self.prev_first_wpt_index = first_wpt_index % num_waypoints_in_list # Prepare for calculating speed: slow_down = False stop = False reached_zero_velocity = False car_distance_to_stop_line = -1. # self.print_wp(self.waypoints.waypoints, first_wpt_index) #self.print_wp(self.waypoints.waypoints, first_wpt_index) # If red light, stop at the red light waypoint if self.redlight_wp != -1: # Find the distance to red light waypoint car_distance_to_stop_line = self.distance_tl( self.waypoints.waypoints, first_wpt_index, self.redlight_wp) # Compare car distance to min distance to make sure enough time to stop if (car_distance_to_stop_line > (STOP_DIST + STOP_TOL)) and (car_distance_to_stop_line <= SLOW_DIST): slow_down = True rospy.loginfo('Slowing for red light') # Use distance and current velocity to solve for average acceleration decel = ((self.current_velocity / (car_distance_to_stop_line - STOP_DIST)) * 0.6) # TODO Add mode to wait at red light # if within stopping distance, set future waypoints velocity to zero elif (car_distance_to_stop_line <= (STOP_DIST + STOP_TOL)) and (car_distance_to_stop_line >= (STOP_DIST - STOP_TOL)): stop = True rospy.loginfo("Stopping at red light") # Fill the lane with the final waypoints for num_wp in range(LOOKAHEAD_WPS): wp = Waypoint() wp.pose = self.waypoints.waypoints[(first_wpt_index + num_wp) % num_waypoints_in_list].pose wp.twist = self.waypoints.waypoints[ (first_wpt_index + num_wp) % num_waypoints_in_list].twist wp_max_velocity = wp.twist.twist.linear.x # rospy.loginfo_throttle(10,'Waypoint max speed: ' + str(wp_max_velocity) + ' - ' + str(self.max_velocity) ) # Find velocity target based on stopping or not if slow_down: # Set all waypoints to same target velocity TODO may need calc each wp velocity wp.twist.twist.linear.x = max( 0.0, self.current_velocity - decel) elif stop: # set velocity to zero wp.twist.twist.linear.x = 0.0 else: # wp.twist.twist.linear.x = wp_max_velocity wp.twist.twist.linear.x = self.max_velocity wp.twist.twist.linear.y = 0.0 wp.twist.twist.linear.z = 0.0 wp.twist.twist.angular.x = 0.0 wp.twist.twist.angular.y = 0.0 wp.twist.twist.angular.z = 0.0 lane.waypoints.append(wp) # finally, publish waypoints as modified on /final_waypoints topic self.final_waypoints_pub.publish(lane)
def publish_waypoints(self): closest_wp_idx = self.closest_wp_idx start_point_velocity = self.get_waypoint_velocity( self.waypoints[closest_wp_idx]) # get the distance from each waypoint till the stop line distance_to_stop_line = self.distances_to_end( self.waypoints[closest_wp_idx:self.stop_idx]) farthest_wp_idx = self.closest_wp_idx + LOOKAHEAD_WPS lane = Lane() waypoints_ahead = [] print(self.driving_state) if self.driving_state == DRIVE_STATE_STOPPING and closest_wp_idx < self.stop_idx: # Loop over the waypoints up to next_red_light waypoint # setting desired velocity at each waypoint as a ratio of remaining distance to stop # from the respective waypoint to the total distance from current position # to stop line for i in range(0, (abs(closest_wp_idx - self.stop_idx))): # get the distance to the i-th way point # i_point_distance = self.distances_to_end(self.waypoints, self.closest_waypoint, i) p = Waypoint() p.pose = self.waypoints[closest_wp_idx + i].pose p.twist = self.waypoints[closest_wp_idx + i].twist if (distance_to_stop_line[0]) > STOP_LINE_OFFSET: if self.current_velocity < 0.8: i_point_target_velocity = -10.0 else: i_point_target_velocity = distance_to_stop_line[ i] / distance_to_stop_line[0] i_point_target_velocity = (start_point_velocity * i_point_target_velocity) else: i_point_target_velocity = -10.0 # negative stops car 'creep' when stopped p.twist.twist.linear.x = i_point_target_velocity waypoints_ahead.append(self.waypoints[closest_wp_idx + i]) for i in range(self.stop_idx, farthest_wp_idx): waypoints_ahead.append(self.waypoints[i]) lane.waypoints = waypoints_ahead elif self.driving_state == DRIVE_STATE_DRIVING: # Set the velocity to reference velocity if driving state is not stopping # print("in else") lane.waypoints = self.waypoints[closest_wp_idx:farthest_wp_idx] # print(lane.waypoints) else: pass # now publish the waypoints # get LOOKAHEAD_WPS number of waypoints ahead of the car # n_waypoints = len(self.waypoints) # can only get this many waypoints # if n_waypoints > LOOKAHEAD_WPS: # n_waypoints = LOOKAHEAD_WPS # max waypoints to pass over # for i in range(n_waypoints): # # check that the waypoints we want are in the range of the waypoint array # if closest_wp_idx + i < len(self.waypoints): # waypoints_ahead.append(self.waypoints[closest_wp_idx + i]) # structure the data to match the expected styx_msgs/Lane form # lane.waypoints = waypoints_ahead # list of waypoints ahead of the car # lane.header.stamp = rospy.Time(0) # timestamp self.final_waypoints_pub.publish(lane)
def _get_waypoint(self, value=0.0): waypoint = Waypoint() waypoint.pose = self._get_posestamped(value) waypoint.twist = self._get_twiststamped(0.0) return waypoint
def pose_cb(self, msg): self.pose = msg first_wpt_index = -1 min_wpt_distance = float('inf') if self.waypoints is None: return num_waypoints_in_list = len(self.waypoints.waypoints) # Gererate an empty lane to store the final_waypoints lane = Lane() lane.header.frame_id = self.waypoints.header.frame_id lane.header.stamp = rospy.Time(0) lane.waypoints = [] # Iterate through the complete set of waypoints until we found the closest distance_decreased = False #rospy.loginfo('Started at waypoint index: %s', self.prev_first_wpt_index) #start_time = time.time() for index, waypoint in enumerate( self.waypoints.waypoints[self.prev_first_wpt_index:] + self.waypoints.waypoints[:self.prev_first_wpt_index], start=self.prev_first_wpt_index): current_wpt_distance = self.distance(self.pose.pose.position, waypoint.pose.pose.position) if distance_decreased and current_wpt_distance > min_wpt_distance: break if current_wpt_distance > 0 and current_wpt_distance < min_wpt_distance: min_wpt_distance = current_wpt_distance first_wpt_index = index distance_decreased = True first_wpt_index %= num_waypoints_in_list transformed_light_point = None if first_wpt_index == -1: rospy.logwarn( 'WaypointUpdater::waypoints_cb - No waypoints ahead of ego were found... seems that the car went off course' ) else: #transform fast avoiding wait cycles # Transform first waypoint to car coordinates self.waypoints.waypoints[ first_wpt_index].pose.header.frame_id = self.waypoints.header.frame_id try: self.tf_listener.waitForTransform("base_link", "world", rospy.Time(0), rospy.Duration(0.02)) transformed_waypoint = self.tf_listener.transformPose( "base_link", self.waypoints.waypoints[first_wpt_index].pose) except (tf.Exception, tf.LookupException, tf.ConnectivityException): try: self.tf_listener.waitForTransform( "base_link", "world", rospy.Time(0), rospy.Duration(TIMEOUT_VALUE)) transformed_waypoint = self.tf_listener.transformPose( "base_link", self.waypoints.waypoints[first_wpt_index].pose) except (tf.Exception, tf.LookupException, tf.ConnectivityException): rospy.logwarn("Failed to find camera to map transform") return # All waypoints in front of the car should have positive X coordinate in car coordinate frame # If the closest waypoint is behind the car, skip this waypoint if transformed_waypoint.pose.position.x <= 0.0: first_wpt_index += 1 self.prev_first_wpt_index = first_wpt_index % num_waypoints_in_list # Prepare for calculating velocity: slow_down = False reached_zero_velocity = False car_distance_to_stop_line = -1. planned_velocity = self.default_velocity # If the last traffic_waypoint message is newer than the threshold, we might need to the car. if self.light_waypoint_index >= 0: rospy.logdebug('should stopp the car %s', self.light_waypoint_index) self.waypoints.waypoints[ self. light_waypoint_index].pose.header.frame_id = self.waypoints.header.frame_id transformed_light_point = self.tf_listener.transformPose( "base_link", self.waypoints.waypoints[self.light_waypoint_index].pose) # The approximate distance from the stop line to the traffic light car_distance_to_stop_line = transformed_light_point.pose.position.x - self.light_distance_thresh # Estimate whether the car cannot cross the stop line on yellow (in less than 2 seconds). Otherwise don't slow down. if self.velocity / car_distance_to_stop_line < 2 and car_distance_to_stop_line >= 4: slow_down = True if self.car_distance_to_sl_when_car_started_to_slow_down is None: self.car_distance_to_sl_when_car_started_to_slow_down = car_distance_to_stop_line self.car_velocity_when_car_started_to_slow_down = self.velocity rospy.logdebug('Stopping the car') planned_velocity = min( max(abs(car_distance_to_stop_line * 0.2), 0.0), self.default_velocity) # Stop the car in a safe distance before the stop line to give the simulator space to adapt velocity #we are close to the stop line and slow elif car_distance_to_stop_line > 0 and car_distance_to_stop_line < 4 and self.velocity < 6: slow_down = True if car_distance_to_stop_line > 0.5: planned_velocity = 1.0 else: planned_velocity = 0.0 reached_zero_velocity = True else: rospy.logwarn('too late to stopp the car') self.car_distance_to_tl_when_car_started_to_slow_down = None self.car_velocity_when_car_started_to_slow_down = None rospy.loginfo( 'car_distance_to_stop_line %s velocity %s set to %s', car_distance_to_stop_line, self.velocity, planned_velocity) # Fill the lane with the final waypoints for num_wp in range(LOOKAHEAD_WPS): wp = Waypoint() wp.pose = self.waypoints.waypoints[(first_wpt_index + num_wp) % num_waypoints_in_list].pose wp.twist = self.waypoints.waypoints[ (first_wpt_index + num_wp) % num_waypoints_in_list].twist wp.twist.twist.linear.x = planned_velocity wp.twist.twist.linear.y = 0.0 wp.twist.twist.linear.z = 0.0 wp.twist.twist.angular.x = 0.0 wp.twist.twist.angular.y = 0.0 wp.twist.twist.angular.z = 0.0 lane.waypoints.append(wp) # finally, publish waypoints as modified on /final_waypoints topic self.final_waypoints_pub.publish(lane)