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 - 2, 0) dist = self.distance(waypoints, i, stop_idx) vel = math.sqrt(2 * MAX_DECEL * dist) if vel < 1.: vel = 0. p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) temp.append(p) return temp
def decelerate_waypoints(self,waypoints,closest_idx): # deceleration logic temp=[] for i,wp in enumerate(waypoints): p=Waypoint() #instantiate a waypoint object p.pose=wp.pose stop_idx=max(self.stopline_wp_idx - closest_idx -2, 0) # subtract two way-points to ensure nose of car stops at line dist=self.distance(waypoints,i,stop_idx) # calculate distance from current index to stop line index vel=math.sqrt(2.0*MAX_DECEL*dist) # some deceleration rate based on distance if vel<1.0: vel=0.0 p.twist.twist.linear.x=min(vel,wp.twist.twist.linear.x) #set way point velocity to min of calculated vs speed limit (default value) temp.append(p) return temp
def _decelerate_waypoints(self, base_waypoints, closest_idx): temp = [] for i, wp in enumerate(base_waypoints): p = Waypoint() p.pose = wp.pose # Two base_waypoints back from line so front of car stops at line # (-2 is from the center of the car) stop_idx = max(self.stopline_wp_idx - closest_idx - 2, 0) dist = self.distance(base_waypoints, i, stop_idx) # TODO: consider a different function than sqare root vel = np.power(MAX_DECEL * dist * 7, 0.33) * 1.7 if vel < 1.0: vel = 0 p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) temp.append(p) return temp
def set_velocity(waypoints, start_index, wp_index_to_stop): #[Waypoint] # rospy.loginfo("start_index: %d, wp_index_to_stop: %d" % (start_index, wp_index_to_stop)) if wp_index_to_stop == -1 or wp_index_to_stop >= start_index + len( waypoints): # use the original velocity of the base waypoints return waypoints wps_with_vel = [] for wp_i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose stop_wp_index = max(wp_index_to_stop - start_index - 2, 0) dist_to_stop = WaypointUpdater.distance(waypoints, wp_i, stop_wp_index) vel = math.sqrt(2. * MAX_DECEL * dist_to_stop) if vel < 1.: vel = 0. p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) wps_with_vel.append(p) return wps_with_vel
def decelerate_waypoints(self, waypoints, closest_idx): temp = [] for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose # Two waypoints back from the line so front of the car stops at line stop_idx = max(self.stopline_wp_index - closest_idx - 2, 0) # Decelerate linearly # velocity = dist / 5 # Decelerate by S-curve # velocity = wp.twist.twist.linear.x * (1. - 1. / (1. + math.exp(-(i - stop_idx + 30.) / 6.))) dist = self.distance(waypoints, i, stop_idx) velocity = math.sqrt(2 * MAX_DECEL * dist) if velocity < 1.: velocity = 0. p.twist.twist.linear.x = min(velocity, wp.twist.twist.linear.x) temp.append(p) return temp
def decelerate_waypoints(self,waypoints,closest_idx): temp = [] stop_idx = max(self.next_light_wp - closest_idx -5,0) #rospy.loginfo("stop index is %s",stop_idx) #waypoints = self.set_velocity(waypoints,0.) #rospy.loginfo("i is %s", i) #rospy.loginfo("stop_idx is %s", stop_idx) #stop index reached. stop the vehicle. for i , wp in enumerate(waypoints): #waypoint_till_traffic_light = stop_idx - i #rospy.loginfo("waypoint till traffic is %s", waypoint_till_traffic_light) #rospy.loginfo(" i is %s" , i) p = Waypoint() p.pose = wp.pose dist = self.distance(waypoints,i,stop_idx) #rospy.loginfo("i is less than stop index") #rospy.loginfo("dist is %s",dist) vel = K_CONSTANT * dist if vel < 1.: vel = 0. p.twist.twist.linear.x = min(vel,wp.twist.twist.linear.x) #rospy.loginfo("final velocity is %s", p.twist.twist.linear.x) temp.append(p) return temp
def process(self): # TODO: Use self.final_waypoints_pub to publish the next target waypoints # In phase 1: we can ignore traffic lights and simply output the next N waypoints *ahead of the car*, with their default velocity # In phase 2: you need to adjust target speeds on waypoints in order to smoothly brake until the car reaches the waypoint # corresponding to the next red light's stop line (stored in self.next_traffic_light_stopline_index, == -1 if no next traffic light). # Advice: make sure to complete dbw_node and have the car driving correctly while ignoring traffic lights before you tackle phase 2 msg = Lane() closest_idx = self.waypoints_db.get_next_closest_idx( self.current_car_position) farthest_idx = closest_idx + self.N base_waypoints = self.waypoints_db.waypoints[closest_idx:farthest_idx] if self.next_traffic_light_stopline_index == -1 or ( self.next_traffic_light_stopline_index >= farthest_idx): msg.waypoints = base_waypoints else: new_points = [] for i, wp in enumerate(base_waypoints): point = Waypoint() point.pose = wp.pose stop_idx = max( self.next_traffic_light_stopline_index - closest_idx - 4, 0) dist = self.waypoints_db.distance(base_waypoints, i, stop_idx) vel = math.sqrt(2 * self.max_deceleration * 0.90 * dist) if vel < 1.0: vel = 0.0 point.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) new_points.append(point) msg.waypoints = new_points self.final_waypoints_publisher.publish(msg)
def decelerate_waypoints(self, base_wp, closest_idx): # use a temporary list to NOT modify the real base waypoints (only published once) temp_list = [] # stop some waypoints in front of the stopline to avoid the car's nose to go over stop_idx = max(self.stop_line_idx - closest_idx - 4, 0) # for each of the waypoints between us and the stopline, set the velocity for i, wp in enumerate(base_wp): p = Waypoint() p.pose = wp.pose distance = self.distance(base_wp, i, stop_idx) MAX_DECEL = 1 vel = math.sqrt(2 * MAX_DECEL * distance) if vel < 1.: vel = 0. # the current twist value of the base waypoint is the speed limit -- no speeding here! p.twist.twist.linear.x = min(vel, base_wp[i].twist.twist.linear.x) temp_list.append(p) return temp_list
def accelerate_waypoints(self, waypoints): temp = [] last_vel = self.current_vel last_time_to_next_wp = 0.0 #rospy.logwarn("Waypoint length: {0}".format(len(waypoints))) for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose time_to_next_wp = 0.0 dist = 0.0 # To calculate accel to be added, it is necessary to determine the time it will # take at the current velocity to reach the next waypoint if i == len(waypoints) - 1: new_vel = last_vel + last_time_to_next_wp * self.max_accel else: dist = self.distance(waypoints, i, i + 1) if last_vel < 1.0: new_vel = 1.0 else: time_to_next_wp = dist / last_vel new_vel = last_vel + (self.max_accel * time_to_next_wp) #rospy.logwarn("Last velocity: {0}, New velocity: {1}, TTNWP: {2}, Dist: {3}".format(last_vel, new_vel, time_to_next_wp, dist)) new_vel = min(new_vel, wp.twist.twist.linear.x) last_vel = new_vel last_time_to_next_wp = time_to_next_wp p.twist.twist.linear.x = new_vel temp.append(p) return temp
def decelerate_waypoints(self, base_waypoints, closest_idx): temp = [] for i, wp in enumerate(base_waypoints): p = Waypoint() p.pose = wp.pose stop_idx = max(self.stopline_wp_idx - closest_idx - 5, 0) # Car is 5 wp long dist = self.distance(base_waypoints, i, stop_idx) # https://physics.stackexchange.com/questions/3818/stopping-distance-frictionless vel = math.sqrt(2 * MAX_DECEL * dist) # velocity decreases as dist decreases if vel < 1.: vel = 0. p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) temp.append(p) return temp
def decelerate_waypoints(self, waypoints, closest_idx): temp = [] for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose # Distance includes a number of waypoints back so front of car stops at line stop_idx = max(self.stopline_wp_idx - closest_idx - STOP_LINE_MARGIN, 0) dist = self.distance(waypoints, i, stop_idx) vel = math.sqrt(2 * 0.5 * dist) + (i * DECEL) if vel < 1.0: vel = 0.0 p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) temp.append(p) self.decelerate_count += 1 if (self.decelerate_count % THROTTLE_NO) == 0: size = len(waypoints) - 1 vel_start = temp[0].twist.twist.linear.x vel_end = temp[size].twist.twist.linear.x rospy.logwarn("DECEL: vel[0]={:.2f}, vel[{}]={:.2f}".format(vel_start, size, vel_end)) return temp
def decelerate_waypoints(self, waypoints, stop_idx): waypoints_up_to_stop = waypoints[:stop_idx] dists_to_stop = self.distances_to_end(waypoints_up_to_stop) result = [] for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose if i >= stop_idx: vel = 0 else: dist_to_stop = dists_to_stop[i] # Linear decrease in velocity wrt time. We could smoothen this. vel = math.sqrt(2 * MAX_DECEL * dist_to_stop) # obey speed limit vel = min(vel, wp.twist.twist.linear.x) p.twist.twist.linear.x = vel result.append(p) return result
def decelerate_waypoints(self, waypoints, closest_idx): temp = [] stop_idx = max( self.stopline_wp_idx - closest_idx - 4, 0 ) # Four waypoints back from stop line so front of car stops before stop line for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose braking_vel = 0.0 dist = self.distance(waypoints, i, stop_idx) if dist > 0.0: time_to_complete_stop = math.sqrt(dist * 2.0 / MAX_DECEL) braking_vel = 2.0 * dist / time_to_complete_stop if braking_vel < 0.1: braking_vel = 0.0 p.twist.twist.linear.x = min(braking_vel, wp.twist.twist.linear.x) temp.append(p) return temp
def decelerate_waypoints(self, waypoints, closest_index): """ Function : Decelerate the car if red light and stop close to it Input Args : Waypoints and closest Red light index Output : temporary deceleration waypoints """ rospy.logerr("In Decelerate waypoints") temp_waypoints = [] print("Waypoints : ", waypoints) print("Closest WP index : ", closest_index) for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose stop_index = max( self.red_trafficlight_waypoint - closest_index - 3, 0) distance = self.distance(waypoints, i, stop_index) velocity = math.sqrt( 2 * DECELARATION_LIMIT * distance ) #+ (i * SMOOTH_DECEL) #SMOOTH_DECEL needed for smoother slowing # Can also implement a S curve implementation here if velocity < 1.0: velocity = 0. p.twist.twist.linear.x = min(velocity, wp.twist.twist.linear.x) temp_waypoints.append(p) self.decelerate_count += 1 if (self.decelerate_count % 100) == 0.0: wp_size = len(waypoints) - 1 velocity_start = temp_waypoints[0].twist.twist.linear.x velocity_end = temp_waypoints[wp_size].twist.twist.linear.x # Logging the messages for information and documentation return temp_waypoints
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 decelerate_waypoints(self, waypoints, closest_idx): temp = [] stop_idx = max( self.stopline_wp_idx - closest_idx - self.stop_waypoint_margin, 0 ) # Two waypoints back from stop line so front of car stops before stop line last_braking_velocity = self.current_vel new_dec_rate = TARGET_DECEL_RATE #rospy.logwarn("") #rospy.logwarn("*** Calculating decel waypoints: Stopline idx: {0}, Closest idx:{1}, Stop idx: {2}".format(self.stopline_wp_idx, closest_idx, stop_idx)) for i, wp in enumerate(waypoints): p = Waypoint() p.pose = wp.pose braking_vel = 0.0 dist = self.distance(waypoints, i, stop_idx) # ** Maybe change this dist value to dist > 1.0 to prevent rolling forward? if dist > 3.0 and last_braking_velocity < 1.0: # Allow vehicle to accelerate from a dead stop if it is away from the stop line # This is needed to get the car to start moving if the traffic light is red # Calculate time to decelerate for the current distance at the deceleration rate tts = math.sqrt(dist * 2.0 / new_dec_rate) # Calculate braking velocity (a* t) braking_vel = new_dec_rate * tts # Clamp to maximum velocity parameter braking_vel = min(braking_vel, wp.twist.twist.linear.x) elif dist > 0.0 and not last_braking_velocity == 0.0: tts = 0.0 if i == 0: # Determine if the vehicle needs to decelerate at a rate faster than # the TARGET_DECEL_RATE, which is typically caused by: # - Missed classifications that cause a light state change # - Late detection of a stop light # - YELLOW to RED light transitions # Calculate time to decelerate at the last known velocity tts = dist * 2.0 / last_braking_velocity # Calculate the deceleration rate needed to stop the vehicle from the # last known velocity and time to stop req_dec_rate = last_braking_velocity / tts # Grab the higher decel rate - This will prevent pre-mature deceleration req_dec_rate = max(req_dec_rate, TARGET_DECEL_RATE) # Clamp the decel rate to the MAX_ACCEL parameter new_dec_rate = min(req_dec_rate, self.max_decel) else: # Continue to decelerate at the last calculated decel rate # Calculate time to decelerate for the current distance at the # deceleration rate tts = math.sqrt(dist * 2.0 / new_dec_rate) # Calculate braking velocity (a * t) braking_vel = new_dec_rate * tts # Clamp to maximum velocity parameter braking_vel = min(braking_vel, wp.twist.twist.linear.x) # Hold car at a stop if the velocity is below 1.0 m/s if braking_vel < 0.5: braking_vel = 0.0 # Set last braking velocity to the new velocity so that this IF block can # be bypassed on the rest of the waypoints last_braking_velocity = braking_vel # rospy.logwarn("Curr Vel: {0}, Braking Vel: {1}, Dist: {2}, New Decel Rate: {3}, \ # TTS: {4}" \ # .format(last_braking_velocity, braking_vel, dist, new_dec_rate, \ # tts)) # Original function #vel = math.sqrt(2 * MAX_DECEL * dist) #if vel < 0.5: # vel = 0.0 p.twist.twist.linear.x = braking_vel temp.append(p) return temp
def loop(self): rate = rospy.Rate(REFRESH_RATE) while not rospy.is_shutdown(): rate.sleep() if self.current_waypoints is None or self.current_pose is None: continue next_waypoint_index = self.next_infront_waypoint() # set the target speed target_speed = self.max_speed # set the number of waypoints received from /base_waypoints number_waypoints = len(self.current_waypoints) lane = Lane() lane.header.frame_id = self.current_pose.header.frame_id lane.header.stamp = rospy.Time(0) if (self.traffic_stop_waypoint_index != -1 and self.is_in_braking_distance()): # we have decided that the waypoint published by the /traffic_waypoint # is where we need to stop the car at stop_waypoint_index = self.traffic_stop_waypoint_index # TODO: handle wrapping # Do we always generate LOOKAHEAD_WPS number of points? what to do # if we will have points after the red traffic light? Just continue but # set the velocity to 0 for those points? decelerate_points_count = stop_waypoint_index - self.next_waypoint_index if decelerate_points_count > 0: for i in range(decelerate_points_count): wp_new = Waypoint() wp_extract = self.current_waypoints[ next_waypoint_index] wp_new.pose = wp_extract.pose lane.waypoints.append(wp_new) wp_new.twist.twist.linear.x = target_speed next_waypoint_index = (next_waypoint_index + 1) % number_waypoints lane.waypoints = self.decelerate(lane.waypoints) #generate up to the LOOKAHEAD_WPS number of waypoints #fill it up with waypoints with zero velocity if decelerate_points_count < LOOKAHEAD_WPS: for i in range(LOOKAHEAD_WPS - decelerate_points_count): wp_new = Waypoint() wp_extract = self.current_waypoints[ next_waypoint_index] wp_new.pose = wp_extract.pose lane.waypoints.append(wp_new) wp_new.twist.twist.linear.x = 0 else: # now create the waypoints ahead for i in range(LOOKAHEAD_WPS): # create a new waypoint, rather than ammending existing waypoints wp_new = Waypoint() # extract the desired waypoint, starting at the next_waypoint_index wp_extract = self.current_waypoints[next_waypoint_index] # copy the position contents of the base_waypoint to the new waypoint wp_new.pose = wp_extract.pose # set the target velocity of the new waypoint wp_new.twist.twist.linear.x = target_speed # add to the Lane waypoints list lane.waypoints.append(wp_new) # then increment the next_waypoint_index, considering circlic nature of list next_waypoint_index = (next_waypoint_index + 1) % number_waypoints lane.waypoints = self.accelerate(lane.waypoints) self.final_waypoints_pub.publish(lane)
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_waypoints(self, idx): idx = max(0, idx) end_idx = min(idx + LOOKAHEAD_WPS, len(self.base_waypoints.waypoints) - 1) stopline_wp_idx = self.stopline_wp_idx stopping = False if self.published_waypoints is not None and self.last_stopline_wp_idx == stopline_wp_idx\ and abs(idx - self.published_waypoints_offset) < 5: #reuse and extend the waypoints used_up = idx - self.published_waypoints_offset self.published_waypoints = self.published_waypoints[used_up:] self.published_waypoints_offset = idx if len(self.published_waypoints) < LOOKAHEAD_WPS: #cruising and we should copy over some waypoints first_copied = idx + len(self.published_waypoints) if first_copied < len(self.base_waypoints.waypoints): self.published_waypoints.extend( self.base_waypoints.waypoints[first_copied:end_idx]) else: #either stopping, standing still or starting or manual control current_velocity = self.current_velocity self.last_stopline_wp_idx = stopline_wp_idx if stopline_wp_idx > idx: end_idx = stopline_wp_idx dist_stop = max( self.distance(self.base_waypoints.waypoints, idx, end_idx), 2) x = [dist_stop] y = [current_velocity] stopping = True x.append(0.75 * dist_stop) y.append(0.75 * current_velocity) x.append(0.1 * dist_stop) y.append(0.15 * current_velocity) if 0.1 * dist_stop > 4.0 and 0.75 * dist_stop > 4.0: x.append(4.0) y.append(0.0) x.append(0.0) y.append(0.0) x.append(-1) y.append(0.0) else: start_dist = max( self.distance(self.base_waypoints.waypoints, idx, end_idx), 2) x = [start_dist + 1, start_dist] current_velocity = max(current_velocity, 2) y = [current_velocity, current_velocity] target_velocity = min( self.MAX_VELOCITY, self.base_waypoints.waypoints[end_idx]. twist.twist.linear.x) diff_vel = target_velocity - current_velocity safe_accel = 0.6 * self.MAX_ACCEL t = abs(diff_vel / safe_accel) dist_t = start_dist - ( t * current_velocity + math.copysign(0.5 * safe_accel, diff_vel) * t**2) x.append(dist_t) y.append(target_velocity) if dist_t < 0: x.append(dist_t - 10) y.append(target_velocity) else: x.append(0) y.append(target_velocity) x.append(-1.0) y.append(y[-1]) x.reverse() y.reverse() fixed_speed = False rospy.logwarn( "waypoint_updater: stopping:{0} fixed_speed:{1} stopline_idx:{2} x:{3} y:{4}" .format(stopping, fixed_speed, stopline_wp_idx, x, y)) if (not stopping and abs(y[-1] - y[0]) < 0.5): fixed_speed = y[0] else: spline_rep = interpolate.splrep(x, y, s=0.1) new_wps = [] log_vel = [] for i in range(idx, end_idx): p = Waypoint() p.pose = self.base_waypoints.waypoints[i].pose dist = self.distance(self.base_waypoints.waypoints, i, end_idx) if fixed_speed: vel = fixed_speed else: vel = interpolate.splev(dist, spline_rep, der=0).sum() if stopping and abs(dist) < 4: vel = 0.0 elif vel < 0.1: vel = 0.0 p.twist.twist.linear.x = vel log_vel.append(vel) new_wps.append(p) self.published_waypoints = new_wps self.published_waypoints_offset = idx rospy.logwarn("waypoint_updater: velocities:{0} ".format(log_vel)) lane = Lane() lane.waypoints = self.published_waypoints self.final_waypoints_pub.publish(lane)
def generate_waypoint(self, waypoint, vel_cmd): p = Waypoint() p.pose = waypoint.pose p.twist.twist.linear.x = vel_cmd return p
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)
def publish_waypoints(self, start): ''' Publishes a message of type :py:class:Lane containing the points ahead the last know pose :py:attr:pose. ''' lane = Lane() #closest_index = self.get_closest_waypoints() closest_index = start farthest_index = closest_index + LOOKAHEAD_WPS rospy.loginfo( "closest_index = %d farthest_index = %d self.stopline_waypoint_index = %d", closest_index, farthest_index, self.stopline_waypoint_index) # rospy.loginfo("closest_index = %d", self.linear_search()) lane.header = self.base_waypoints.header if self.stopline_waypoint_index == -1 or (self.stopline_waypoint_index >= farthest_index): lane.waypoints = self.base_waypoints.waypoints[ closest_index:farthest_index] # rospy.loginfo("farthest_index=%d length=%d", farthest_index, len(self.base_waypoints.waypoints)) # for i in range(len(lane.waypoints)): # velocity = lane.waypoints[i].twist.twist.linear.x # # rospy.loginfo("Velocity=%2.2f", velocity) else: # decelerate lane.waypoints = [] # stop a little before the stop line to account for falf of the length of the car stop_index = max(self.stopline_waypoint_index - 2, 0) # try to stop smoothly in 30 waypoints from the stop line v_index = max(0, stop_index - 30) v0 = self.get_waypoint_velocity( self.base_waypoints.waypoints[v_index]) x0 = self.distance(self.base_waypoints.waypoints, v_index, stop_index) / 2.0 rospy.loginfo("x0=%f v0=%f", x0, v0) for i in range(closest_index, stop_index): waypoint = Waypoint() waypoint.pose = self.base_waypoints.waypoints[i].pose # calculate velocity as a sgmoid function dist = self.distance(self.base_waypoints.waypoints, i, stop_index) exp_term = math.exp(-0.2 * (dist - x0)) velocity = v0 / (1 + exp_term) if (i == stop_index - 1): velocity = 0.0 # do not exceed logitudinal acceleration limits max_vel = math.sqrt(2 * self.decel_limit_mpsps * dist) if max_vel < 1.: max_vel = 0.0 rospy.loginfo("dist=%2.2f velocity=%2.2f max_vel=%2.2f", dist, velocity, max_vel) velocity = min(max_vel, velocity) waypoint.twist.twist.linear.x = min( velocity, self.base_waypoints.waypoints[i].twist.twist.linear.x) lane.waypoints.append(waypoint) #set the velocity of the last point to 0.0 #lane.waypoints[ len(lane.waypoints) - 1 ] = 0.0 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 copy_waypoint_pose(wp): new_wp = Waypoint() new_wp.pose = wp.pose new_wp.twist.twist.linear.x = wp.twist.twist.linear.x return new_wp
def decelerate_waypoints(self, waypoints, closest_idx): print("Waypoints: ") # Make a new list of waypoints in order to store the manipulated base waypoints # temp = [] # Two waypoints back from line so front of car stops approximately at the line # stop_idx = max(self.stopline_wp_idx - closest_idx - 2, 0) # Unused attempts at linear decelleration # # Calculate the distance between the first waypoint and the index at which the vehicle has to come to a standstill # dist_tot = self.distance(waypoints, 0, stop_idx) v_start = self.current_vel # self.pose.twist.twist.linear.x # self.distance(waypoints, 0, stop_idx) # Calculate the time required to come to a standstill at the stop index assuming linear unlimited acceleration # #t_tot = 2.0 * dist_tot / v_start # Calculate the necessary acceleration ("deceleration") across the entire section # #accel = -1.0 * min(MAX_DECEL, ( v_start / t_tot ) ) # Calculate the distance to come to a standstill # #dist_stop = v_start*v_start / ( 2.0 * (-1.0 * accel) ) # Calculate the corresponding acceleration per meter # #accel_m = v_start / dist_stop #a_would_be = v_start * v_start / ( 2.0 * dist_tot ) #if MAX_DECEL <= a_would_be: #for i in range(stop_idx, len(waypoints)): #d_would_be = self.distance(waypoints, 0, i) #a_would_be = v_start * v_start / ( 2.0 * d_would_be ) #if a_would_be <= MAX_DECEL: #stop_idx = i #break #d_stop = v_start * v_start / ( 2.0 * a_would_be ) #a_d = v_start / d_stop # -------------------------------------- # # Calculate the corresponding acceleration per waypoint # for i, wp in enumerate(waypoints): p = Waypoint() # The goal position and orientation of the vehicle at the waypoint remains the same # p.pose = wp.pose # Calculate the distance between this waypoint and the index at which the vehicle has to come to a standstill # dist = self.distance(waypoints, i, stop_idx) # Calculate the velocity the vehicle has to have at this waypoint to come to a standstill in time # #vel = math.sqrt(2.0 * MAX_DECEL * dist) #vel = v_start - ( (dist_tot - dist) * accel_m ) #vel = v_start - ( a_d * (d_stop-dist) ) delta_dist = dist_tot - dist if 0.0 < delta_dist: vel = v_start * (1 - (delta_dist / dist_tot) * (delta_dist / dist_tot)) else: vel = v_start if vel < 0.1: vel = 0.0 # A safeguard to arrive at the destination despite of an over-enthusiastic controller # if i < (stop_idx - 2) and v_start < 0.2: vel = 0.75 #if i == stop_idx - 5: #vel = 0.3 #else: #if i == stop_idx - 4: #vel = 1.0 #else: #vel = 1.5 else: if (stop_idx - 2) < i: vel = 0.0 # Use the calculated required waypoint velocity in stead of the original/base waypoint velocity, unless the base waypoint velocity is smaller than this value # p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) temp.append(p) return temp
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)