def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header #slice the base waypoints from closest idx to LOOKAHEAD_WPS + closest_idx lane.waypoints = self.base_waypoints.waypoints[ closest_idx:closest_idx + LOOKAHEAD_WPS] self.final_waypoints_pub.publish(lane)
def publish_waypoints(self, closest_ind): #hy 6/29 lane = Lane() lane.header = self.base_waypoints.header far_ind = closest_ind + LOOKAHEAD_WPS lane.waypoints = self.base_waypoints.waypoints[closest_ind:far_ind] if self.stopline_wp_ind != -1 and self.stopline_wp_ind < far_ind: # need to slow down temp = [] for i, wp in enumerate(lane.waypoints): p = Waypoint() p.pose = wp.pose stop_ind = max(self.stopline_wp_ind - closest_ind - 2, 0) dist = self.distance(lane.waypoints, i, stop_ind) vel = math.sqrt(2 * MAX_DECEL * dist) if vel < 1.0: vel = 0.0 #set velocity #vel = 0 p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) temp.append(p) lane.waypoints = temp self.final_waypoints_pub.publish(lane)
def construct_final_waypoints(self, start_idx): end_idx = start_idx + LOOKAHEAD_WPS final_waypoints = Lane() final_waypoints.header = self.base_waypoints.header final_waypoints.waypoints = self.base_waypoints.waypoints[ start_idx:end_idx] return final_waypoints
def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header stopline_idx = self.stopline_waypoint_idx if stopline_idx is not None and stopline_idx > closest_idx and stopline_idx < closest_idx + LOOKAHEAD_WPS * 2: if not self.message_decelerate: self.message_decelerate = True rospy.loginfo( "[WPU] decelerate because traffic light is red in {0}m". format(self.distance(closest_idx, stopline_idx))) for i in range(closest_idx, closest_idx + LOOKAHEAD_WPS): dist = self.distance(i, stopline_idx) dist -= 6 # m - substract distance from the center of the vehicle to the stop line speed = self.calc_velocity_for_distance_to_stop( dist, self.max_deceleration) speed = max(min(speed, self.max_speed), 0.) # limit speed between 0 and max_speed self.set_waypoint_velocity(i, speed) else: if self.message_decelerate: self.message_decelerate = False rospy.loginfo("[WPU] accelerate because the way is green") for i in range(closest_idx, closest_idx + LOOKAHEAD_WPS): self.set_waypoint_velocity(i, self.max_speed) lane.waypoints = self.base_waypoints.waypoints[ closest_idx:closest_idx + LOOKAHEAD_WPS] self.final_waypoints_pub.publish(lane)
def publish_waypoints(self,closest_idx): lane = Lane() lane.header = self.base_waypoints.header # The waypoints for that lane should be the waypoints from your base waypoints but # sliced so that it goes from your closest index to the closest index plos hover many LOOKAHEAD is defined lane.waypoints = self.base_waypoints.waypoints[closest_idx:closest_idx + LOOKAHEAD_WPS] self.final_waypoints_pub.publish(lane)
def generate_lane(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header farthest_idx = closest_idx + LOOKAHEAD_WPS new_waypoints = self.base_waypoints.waypoints[closest_idx:farthest_idx] # Case 1: Traffic light is red if (0 < self.stopline_wp_idx < farthest_idx): # Build new waypoints with speed adjusted for environment temp = [] # Lower speed to stop at the line (model deceleration wanted) for i, wp in enumerate(new_waypoints): # Calc velocity curve to smoothly stop the car # Four points back so car stops at line stop_idx = max(self.stopline_wp_idx - closest_idx - 4, 0) dist = self.distance(new_waypoints, i, stop_idx) vel = 0.4 * dist # arbitrary value to work with MAX_DECEL if vel < 0.5: vel = 0. temp.append(self.generate_waypoint(wp, min(vel, wp.twist.twist.linear.x))) lane.waypoints = temp # Case 2: Nothing in the way, raise speed to match waypoint command else: lane.waypoints = new_waypoints # rospy.logwarn("VEL_CMD: {}".format(temp[0].twist.twist.linear.x)) return lane
def publish_red_light_waypoints(self, closest_idx, red_light_waypoint_idx): ## lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = self.base_waypoints.waypoints[ closest_idx:red_light_waypoint_idx + RED_LIGHT_LOOKAHEAD_WPS] len_tl_wps = red_light_waypoint_idx - closest_idx len_wps = len(lane.waypoints) # including LOOK d = self.distance(lane.waypoints, 0, len_tl_wps - 1) #rospy.logwarn("%s, %s, %s", closest_idx, red_light_waypoint_idx, d) for idx in range(len_tl_wps): if (d > MIN_TRAFFIC_LIGHT_DIST and d < MAX_TRAFFIC_LIGHT_DIST ): # change speed within 10~40 mile before red light if self.current_vel < 1: # Too slow, keep constant low speed approaching red light v = 1 else: # Too fast, slow down gradually v = self.current_vel * (1 - float(idx) / (len_wps - 1)) else: v = 0 self.set_waypoint_velocity(lane.waypoints, idx, v) for idx in range(len_tl_wps, len_wps): self.set_waypoint_velocity(lane.waypoints, idx, 0) self.final_waypoints_pub.publish(lane)
def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = self.base_waypoints.waypoints[ closest_idx:closest_idx + LOOKAHEAD_WPS] print('publish wps') self.final_waypoints_pub.publish(lane)
def generate_lane(self): lane = Lane() lane.header = self.base_waypoints.header # publish waypoints starting from next closest point closest_idx = self.get_closest_waypoint_idx() final_idx = closest_idx + LOOKAHEAD_WPS # check for wrap around at end of lap base_waypoints = None if final_idx < len(self.base_waypoints.waypoints): base_waypoints = self.base_waypoints.waypoints[ closest_idx:final_idx] else: final_idx = final_idx % len(self.base_waypoints.waypoints) # add beginning of track data on wrap around base_waypoints = self.base_waypoints.waypoints[ closest_idx:] + self.base_waypoints.waypoints[:final_idx] # check for nearby stoplight if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= final_idx): lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def build_lane(self): closest_index = self.find_closest() farthest_index = closest_index + LOOKAHEAD_WPS normal_waypoints = self.base_waypoints.waypoints[ closest_index:farthest_index] lane = Lane() lane.header = self.base_waypoints.header if (self.stop_line == -1) or (self.stop_line >= farthest_index): # override the speed settings in waypoints if the CRUISE_SPD is set to >0 if CRUISE_SPD > 0: lane.waypoints = self.change_cruise_spd(normal_waypoints) else: lane.waypoints = normal_waypoints else: if self.stop_line > LOOKAHEAD_WPS: index_diff = LOOKAHEAD_WPS else: index_diff = self.stop_line # the self.stop_line is a relative index from car to stop, not absolute rospy.logwarn("red light, car will stop at: {}".format(index_diff)) lane.waypoints = self.change_vel(normal_waypoints, index_diff) return lane
def generate_lane(self): lane = Lane() #generate waypoints closest_waypoint_index = self.get_closest_waypoint_index() farthest_waypoint_index = closest_waypoint_index + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[ closest_waypoint_index:closest_waypoint_index + LOOKAHEAD_WPS] #decelerate waypoints if stopline is near if debug: rospy.logwarn('Waypoint -- closest waypoint ' + str(closest_waypoint_index)) rospy.logwarn('Waypoint -- stopline_wp_index ' + str(self.stopline_wp_index)) if self.stopline_wp_index == None or self.stopline_wp_index == -1 or ( self.stopline_wp_index >= farthest_waypoint_index): #do nothing if the stop waypoint is too far lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_waypoint_index) lane.header = self.base_waypoints.header return lane
def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_wp.header lane.waypoints = self.base_wp.waypoints[closest_idx:closest_idx + LOOKAHEAD_WPS] lane.waypoints = lane.waypoints + self.base_wp.waypoints[:LOOKAHEAD_WPS - len(lane. waypoints)] wps_to_stop = self.traffic_wp - closest_idx if self.traffic_wp != -1 and wps_to_stop < LOOKAHEAD_WPS: dist = self.distance(lane.waypoints, 0, wps_to_stop) if dist <= self.braking_distance: if dist < 1: dist = 0 start_velocity = self.velocity * dist / self.braking_distance for i, wp in zip(range(wps_to_stop), lane.waypoints): vel = start_velocity * (wps_to_stop - (i + 1.)) / wps_to_stop wp.twist.twist.linear.x = vel self.final_waypoints_pub.publish(lane) return # Keep going with constant speed for wp in lane.waypoints: wp.twist.twist.linear.x = self.velocity self.final_waypoints_pub.publish(lane)
def gen_lane(self, closest_idx): if ((self.last_lane_wp is not None) and self.last_close_waypoint_idx == closest_idx and self.last_stop_line_wp_idx == self.stop_line_wp_idx): rospy.logwarn("Not updating closest_idx: %s cur_vel : %s", closest_idx, self.cur_vel) return self.last_lane_wp lane = Lane() lane.header = self.base_waypoints.header farthest_idx = closest_idx + LOOKAHEAD_WPS # If Red Light detected if self.is_red_light_ahead(closest_idx): rospy.logdebug("WaypointUpdater : Found a Red Light Ahead!!") # rospy.logwarn("WaypointUpdater : gen_lane : %s", self.stop_line_wp_idx) lane.waypoints = self.decelerate_to_stop(closest_idx, farthest_idx) # lane.waypoints = self.decelerate_to_stop_dummy(closest_idx, farthest_idx); else: # If No Red light Detected #rospy.loginfo("WaypointUpdater : Found a Non Red Light Ahead!!"); #rospy.loginfo("WaypointUpdater : gen_lane : %s",self.stop_line_wp_idx);l lane.waypoints = self.base_waypoints.waypoints[ closest_idx:farthest_idx] self.last_close_waypoint_idx = closest_idx self.last_lane_wp = lane self.last_stop_line_wp_idx = self.stop_line_wp_idx return lane
def generate_lane(self): lane = Lane() lane.header = self.base_waypoints.header look_ahead_wp_max = self.nearest_wp_idx + LOOKAHEAD_WPS base_wpts = self.base_waypoints.waypoints[self.nearest_wp_idx:look_ahead_wp_max] if self.stop_wp == NO_WP or (self.stop_wp >= look_ahead_wp_max): lane.waypoints = base_wpts else: temp_waypoints = [] stop_idx = max(self.stop_wp - self.nearest_wp_idx - STOPLINE, 0) for i, wp in enumerate(base_wpts): temp_wp = Waypoint() temp_wp.pose = wp.pose if stop_idx >= STOPLINE: dist = self.distance(base_wpts, i, stop_idx) # account for system lag if DELAY > 0: delay_s = 1./DELAY else: delay_s = 0 # x = xo + vot + .5at^2, xo = 0 dist += self.get_waypoint_velocity(base_wpts[i])*delay_s+.5*DECEL_RATE*delay_s*delay_s # v^2 = vo^2 + 2*a*(x-xo) # v^2 = 0 + 2*a*(dist) # v = sqrt(2*a*dist) vel = math.sqrt(2*DECEL_RATE*dist) if vel < 1.0: vel = 0.0 else: vel = 0.0 temp_wp.twist.twist.linear.x = min(vel, self.get_waypoint_velocity(base_wpts[0])) temp_waypoints.append(temp_wp) lane.waypoints = temp_waypoints return lane
def copy_wps(self, start_i, count): last_i = min(len(self.waypoints.waypoints), start_i + count) lane = Lane() lane.header = self.waypoints.header lane.waypoints = copy.deepcopy( self.waypoints.waypoints[start_i:last_i]) return lane
def generate_lane(self): # make a new lane msg object lane = Lane() lane.header = self.lane_msg_header closest_idx = self.current_pose_idx farthest_idx = closest_idx + LOOKAHEAD_WPS # get the slice of base waypoints to publish (with or without changing the target velocity.) base_wp_slice = self.base_waypoints[closest_idx:farthest_idx] # rospy.loginfo("WAYPOINT_UPDATER: Car WP: {0} ** Closest light wp: {1}".format( # self.current_pose_idx, self.stop_line_wp_idx)) # rospy.loginfo("****** WAYPOINT_UPDATER: farthest_idx: {0}".format(farthest_idx)) # decelerate = ((self.stop_line_wp_idx != -1) and (self.stop_line_wp_idx < farthest_idx)) # rospy.loginfo("WAYPOINT_UPDATER: decelerate: {0} ** Cond #1: {1} ** Cond #2: {2}".format( # decelerate, (self.stop_line_wp_idx != -1), (self.stop_line_wp_idx < farthest_idx ))) # # if decelerate: # rospy.loginfo("****** WAYPOINT_UPDATER: Decelerating...") # lane.waypoints = self.decelerate_waypoints(base_wp_slice, closest_idx) # else: # lane.waypoints = base_wp_slice if self.stop_line_wp_idx == -1 or self.stop_line_wp_idx >= farthest_idx: # lane.waypoints = base_wp_slice lane.waypoints = self.accelerate_waypoints( base_wp_slice, self.current_velocity_ms, self.max_velocity_ms) else: # rospy.loginfo("****** WAYPOINT_UPDATER: Decelerating...") lane.waypoints = self.decelerate_waypoints(base_wp_slice, closest_idx) return lane
def publish_waypoints_old(self): closest_idx = self.get_closest_waypoint_idx() lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = self.base_waypoints.waypoints[ closest_idx:closest_idx + LOOKAHEAD_WPS] self.final_waypoints_pub.publish(lane)
def generate_lane(self): lane = Lane() lane.header = self.lane.header closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): if verbose: rospy.logerr("No need to decelerate. Stopline waypoint index = " + str(self.stopline_wp_idx) + ", farthest index = " + str(farthest_idx)) lane.waypoints = base_waypoints else: pts = 10 lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) if verbose: rospy.logerr("Original waypoints: \n" + str(np.array(self.waypoints_2d)[closest_idx:closest_idx + pts])) waypoints_2d = np.array([[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] for waypoint in lane.waypoints]) rospy.logerr("Decelerated waypoints: \n" + str(waypoints_2d[:10])) return lane
def publish_waypoints(self, closest_idx): wp_length = len(self.base_waypoints.waypoints) farthest_idx = closest_idx + LOOKAHEAD_WPS lane = Lane() lane.header = self.base_waypoints.header farthest_idx_mod = farthest_idx % wp_length if farthest_idx_mod < closest_idx: base_waypoints = self.base_waypoints.waypoints[ closest_idx:wp_length] + self.base_waypoints.waypoints[ 0:farthest_idx_mod] else: base_waypoints = self.base_waypoints.waypoints[ closest_idx:farthest_idx] # debug print stop line index if not self.last_idx or self.last_idx != self.stopline_wp_idx: self.last_idx = self.stopline_wp_idx rospy.logwarn("self.stopline_wp_idx: {0}".format( self.stopline_wp_idx)) if self.stopline_wp_idx == -1 or self.stopline_wp_idx >= farthest_idx: lane.waypoints = base_waypoints else: lane.waypoints = self.slowdown_waypoints(base_waypoints, closest_idx) self.final_waypoints_pub.publish(lane)
def generate_lane(self): lane = Lane() lane.header = self.base_waypoints.header closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[ closest_idx:farthest_idx] if (self.stopline_wp_idx == -1) or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints rospy.loginfo( 'In generate lane if statement stopline is:{}'.format( self.stopline_wp_idx)) else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) rospy.loginfo( 'In generate lane Else statement stopline is:{}'.format( self.stopline_wp_idx)) return lane
def generate_lane(self): lane = Lane() lane.header = self.base_waypoints.header #moved from previous section to here closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[closest_idx:farthest_idx] #to determine if need to decelerate, need to verify in simulator #testing what happens without if statement maybe stopline the problem # test interesting car stayed forzen swithed to manual drove it a little enaged auto and it stoped so the deceleration part works but not being trigger so problems must be around stopline_wp_idx or fartest_idx #lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) #another test #self.stopline_wp_idx = Int32(self.stopline_wp_idx)#250 # this was close to the real value I want to see can I force it to stop #self.stopline_wp_idx = 292#similr to real value if (self.stopline_wp_idx == -1) or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints #experiment #lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx)# I want to see will this cause it to stop #rospy.loginfo('In generate lane if statement:') rospy.loginfo('In generate lane if statement stopline is:{}'.format(self.stopline_wp_idx)) else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) #rospy.loginfo('In generate lane else statement:') rospy.loginfo('In generate lane Else statement stopline is:{}'.format(self.stopline_wp_idx)) return lane
def update_waypoints(self, idx): """ Publishes the latest look-ahead waypoints """ # Create the header and set its timestamp header = Header() header.stamp = rospy.Time.now() msg = Lane() msg.header = header # Keep the copy of base_waypoints so that you don't have to recompute them # we are using the same base_waypoints when we get multiple messages for stopping # at a stopline. base_waypoints = self.__base_waypoints[idx:idx + LOOKAHEAD_WPS] msg.waypoints = base_waypoints # If you find out that one of the generated waypoints lies on a stop line # that we should be stopping at then start decelerating if self.__stopline_wp_idx != -1 and self.__stopline_wp_idx < ( idx + LOOKAHEAD_WPS): rospy.logdebug('Planning to stop at ' + str(self.__stopline_wp_idx) + ' from total ' + str(idx + LOOKAHEAD_WPS)) msg.waypoints = self.__decelerate(base_waypoints, idx) self.final_waypoints_pub.publish(msg)
def generate_lane(self, idx, pts): lane = Lane() lane.header = self.waypoints_header lookahead = idx + self.traffic_light_lookahead_wps if self.traffic_light_status and self.traffic_light_status.tlwpidx >= 0 \ and self.traffic_light_status.state in (TrafficLight.RED, TrafficLight.YELLOW) \ and self.waypoints.before(idx, self.traffic_light_status.tlwpidx) \ and self.waypoints.before(self.traffic_light_status.tlwpidx, lookahead): lane.waypoints = self.decelerate_waypoints(idx, pts) elif self.start_accel_wp and self.waypoints.distance( self.start_accel_wp, idx) <= self.acceleration_distance: lane.waypoints = self.accelerate_waypoints(idx, pts) return lane else: dv = self.waypoints[idx].twist.twist.linear.x - self.velocity if dv < -self.max_deceleration * 3: lane.waypoints = self.decelerate_waypoints(idx, pts) elif dv > self.max_acceleration * 3: self.start_accel_wp = idx lane.waypoints = self.accelerate_waypoints(idx, pts) return lane else: self.start_decel_wpidx = None lane.waypoints = self.waypoints[idx:idx + pts + 1] if self.loglevel >= 4: rospy.loginfo("Waypoint update %s: velocity: %f", idx, lane.waypoints[0].twist.twist.linear.x) rospy.loginfo(" Waypoint %s: velocity: %f", idx + pts - 1, lane.waypoints[-1].twist.twist.linear.x) if self.velocity < 0.1: self.start_accel_wp = self.pose_wpidx return lane
def get_final_lane(self, closest_wp_idx): """ Updates final lane's waypoints based on traffic light or obstacle waypoint index. :return: lane with waypoints updated with decelerating linear velocity. """ lane = Lane() lane.header = self.base_waypoints.header farthest_wp_idx = closest_wp_idx + _NUM_WAYPOINTS_AHEAD sliced_base_waypoints = self.base_waypoints.waypoints[closest_wp_idx:farthest_wp_idx] # Determine if vehicle is clear from traffic light and obstacle. traffic_light_clear = (self.traffic_light_wp_idx is None or self.traffic_light_wp_idx == -1 or self.traffic_light_wp_idx >= farthest_wp_idx) obstacle_clear = (self.obstacle_wp_idx is None or self.obstacle_wp_idx == -1 or self.obstacle_wp_idx >= farthest_wp_idx) if traffic_light_clear and obstacle_clear: # No traffic light or obstacle detected. lane.waypoints = sliced_base_waypoints else: if not traffic_light_clear and obstacle_clear: # Only traffic light is detected. target_wp_idx = self.traffic_light_wp_idx elif traffic_light_clear and not obstacle_clear: # Only obstacle is detected. target_wp_idx = self.obstacle_wp_idx else: # Both traffic light and obstacle are detected. target_wp_idx = min(self.traffic_light_wp_idx, self.obstacle_wp_idx) lane.waypoints = self.decelerate_waypoints(sliced_base_waypoints, target_wp_idx - closest_wp_idx) return lane
def publish_final_waypoints(self): lane = Lane() # TODO what do I do for a header? lane.header = self.waypoints.header lane.waypoints = self.final_waypoints self.final_waypoints_pub.publish(lane) pass
def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header farthest_idx = closest_idx + LOOKAHEAD_WPS max_index = len(self.base_waypoints.waypoints) if farthest_idx >= len(self.base_waypoints.waypoints): lane_waypoints = copy.deepcopy( self.base_waypoints.waypoints[closest_idx:]) else: lane_waypoints = copy.deepcopy( self.base_waypoints.waypoints[closest_idx:farthest_idx]) if len(lane_waypoints) < LOOKAHEAD_WPS: lane_waypoints.extend( copy.deepcopy( self.base_waypoints.waypoints[0:LOOKAHEAD_WPS - len(lane_waypoints)])) if self.stopline_wp_idx == -1 or self.stopline_wp_idx - farthest_idx >= 0: lane.waypoints = lane_waypoints else: lane.waypoints = self.decelerate_waypoints(lane_waypoints, closest_idx) #rospy.logwarn('decelerate') self.final_waypoints_pub.publish(lane)
def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = self.base_waypoints.waypoints[ closest_idx:closest_idx + LOOKAHEAD_WPS] self.adjust_velocity(lane.waypoints) self.final_waypoints_pub.publish(lane)
def generate_lane(self): lane = Lane() # Reuse header - Not mandatory lane.header = self.base_waypoints.header # Define the base waypoints from the closest to the farthest closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): # rospy.loginfo("Closest waypoynt idx :{0}".format(closest_idx)) # rospy.loginfo("Farthest waypoynt idx :{0}".format(farthest_idx)) # rospy.loginfo("Normal lane generated") # Case of no stopline in sight - publish base waypoints lane.waypoints = base_waypoints else: # rospy.loginfo("Closest waypoynt idx :{0}".format(closest_idx)) # rospy.loginfo("Farthest waypoynt idx :{0}".format(farthest_idx)) # rospy.loginfo("Decelerating lane generated") # In case a stopline IS in sight, generate decelerating waypoints lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def get_lane(self): # create new lane lane = Lane() # the header is the same lane.header = self.base_waypoints.header # slice waypoints from closest index to the end closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[ closest_idx:farthest_idx] # debug self.closest_idx = closest_idx if (self.stopline_wp_idx == -1) or (self.stopline_wp_idx >= farthest_idx): # no TL data - publish base waypoints lane.waypoints = base_waypoints else: # decelerate to stop before TL lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() lane.header = self.base_lane.header lane.header.frame_id = self.pose.header.frame_id lane.header.stamp = rospy.Time(0) closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS track_waypoint_count = len(self.base_lane.waypoints) if farthest_idx < track_waypoint_count: base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] else: offset = farthest_idx - track_waypoint_count base_waypoints = self.base_lane.waypoints[ closest_idx:track_waypoint_count] base_waypoints[-1].twist.twist.linear.x = 1.0 # Just move on base_waypoints = base_waypoints + self.base_lane.waypoints[0:offset] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): # No traffic light close by within the lookahead limit lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate(base_waypoints, closest_idx) return 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 waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx==-1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = waypoints else: lane.waypoints = self.decelerate_waypoints(waypoints, closest_idx) return lane
def _gen_lane(self, closest_idx): lane = Lane() diff = len(self.waypoints_2d) lane.header = self.base_waypoints.header closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[closest_idx:farthest_idx] if farthest_idx < diff: base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] else: base_waypoints = self.base_lane.waypoints[closest_idx:int(diff)] + self.base_lane.waypoints[0:int(farthest_idx % diff)] 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) return lane
def pose_cb(self, msg): limited_waypoints = [] pos_wp = Waypoint() pos_wp.pose.pose.position.x = msg.pose.position.x pos_wp.pose.pose.position.y = msg.pose.position.y pos_wp.pose.pose.position.z = msg.pose.position.z uptoCount = LOOKAHEAD_WPS - 1 # Since we sent 200 pts last time so the nearest pt could be max at 200 distance self.update_current_postion_wp(pos_wp) total_points_to_send = (self.numOfWaypoints - self.car_pos_index + 1) if self.is_stop_req == 1 and self.car_pos_index >= self.stop_wayp_index: total_points_to_send = 0 if total_points_to_send > 0: if total_points_to_send < uptoCount: uptoCount = total_points_to_send if uptoCount < self.decrement_factor - 10: self.short_of_points = 1 self.stop_wayp_index = self.numOfWaypoints - 1 # The last known index inrange = 0 diff = self.stop_wayp_index - self.car_pos_index if self.car_pos_index <= self.stop_wayp_index: if diff < uptoCount: inrange = 1 num_limited_wp = self.car_pos_index + uptoCount if inrange == 1: num_limited_wp = self.stop_wayp_index # Fill the waypoints for count_index in range(self.car_pos_index, num_limited_wp): if count_index < len(self.rxd_lane_obj.waypoints): pos_wp = self.deep_copy_wp(self.rxd_lane_obj.waypoints[count_index]) limited_waypoints.append(pos_wp) if (self.is_stop_req == 1 and inrange == 1) or self.short_of_points == 1: adv_stop_wap = self.decrement_factor + 1 for i in range(adv_stop_wap): self.velocity_array.append(0) curr_stop_index = self.stop_wayp_index - self.car_pos_index - 2 limited_waypoints = self.prepare_to_stop(limited_waypoints, self.decrement_factor, curr_stop_index) self.last_sent_waypoints = limited_waypoints # Prepare to broadcast lane = Lane() lane.header = self.rxd_lane_obj.header lane.waypoints = limited_waypoints self.final_waypoints_pub.publish(lane) pass
def publish_waypoints(self): lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = self.base_waypoints.waypoints[closest_idx: closest_idx + LOOKAHEAD_WPS] self.final_waypoints_pub.publish(lane)