def generate_lane(self, index): lane = Lane() lookahead_index = index + LOOKAHEAD_WPS waypoints = self.base_lane.waypoints[index:lookahead_index] lane.waypoints = waypoints if not self.is_stopline_ahead( lookahead_index) else self.decelerate_waypoints(waypoints, index) return lane
def generate_Lane(self): lane=Lane() # create lane object closest_idx=self.get_closest_waypoint_id() # get closest way point index farthest_idx=closest_idx+LOOKAHEAD_WPS # calculate farthest index base_waypoints=self.base_lane.waypoints[closest_idx:farthest_idx] # python slicing automatically takes care of end of list at end of way-points if self.stopline_wp_idx==-1 or (self.stopline_wp_idx>=farthest_idx): # stopline way point index comes from the traffic lights subscriber lane.waypoints=base_waypoints else: lane.waypoints=self.decelerate_waypoints(base_waypoints,closest_idx) # need to slow down since there is a traffic light that within the vicinity 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 loop_till_shutdown(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): if self.latest_pose and self.kdtree_orig_waypoints: self.id_closest_waypoint = self.find_id_closest_waypoint() self.id_farthest_waypoint = self.id_closest_waypoint + LOOKAHEAD_WPS final_lane = Lane() final_lane.header = self.orig_waypoints.header sliced_waypoints = self.orig_waypoints.waypoints[ self.id_closest_waypoint:self.id_farthest_waypoint] if (self.id_traffic_waypoint) == -1 or ( self.id_traffic_waypoint >= self.id_farthest_waypoint): final_lane.waypoints = sliced_waypoints else: final_lane.waypoints = self.braking_waypoints( sliced_waypoints) self.final_waypoints_pub.publish(final_lane) rate.sleep()
def generate_lane(self): lane = Lane() 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): # Go ahead lane.waypoints = base_waypoints else: # Stop before stop line lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] stop = 0 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) stop = 1 self.stop_for_tl_pub.publish(stop) return lane
def generate_lane(self): """ In addition to the closest waypoint, a set of subsequent waypoints are added to the lane waypoints output. This method triggers as well the deceleration in case of given stop waypoints """ lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS #base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] base_waypoints = self.base_waypoints.waypoints[ closest_idx:farthest_idx] #We didn't find any traffic lights if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): #publish waypoints directly lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints #rospy.logwarn("Speed up! stopline_wp_idx %d | farthest_idx %d | stopline>fatherst %d"%(self.stopline_wp_idx,farthest_idx, self.stopline_wp_idx >= farthest_idx) ) else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) #rospy.logwarn("Decelerating! stopline_wp_idx %d | farthest_idx %d | stopline>fatherst %d"%(self.stopline_wp_idx,farthest_idx, self.stopline_wp_idx >= farthest_idx) ) return lane
def publish_final_waypoints(self): lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time.now() lane.waypoints = self.final_waypoints rospy.loginfo('1 Waypoint pose: x %d, y %d : Waypoint speed: %d', self.final_waypoints[1].pose.pose.position.x, self.final_waypoints[1].pose.pose.position.y, self.final_waypoints[1].twist.twist.linear.x) rospy.loginfo('100 Waypoint pose: x %d, y %d : Waypoint speed: %d', self.final_waypoints[100].pose.pose.position.x, self.final_waypoints[100].pose.pose.position.y, self.final_waypoints[100].twist.twist.linear.x) rospy.loginfo('199 Waypoint pose: x %d, y %d : Waypoint speed: %d', self.final_waypoints[199].pose.pose.position.x, self.final_waypoints[199].pose.pose.position.y, self.final_waypoints[199].twist.twist.linear.x) self.final_waypoints_pub.publish(lane)
def generate_lane(self): lane = Lane() # Get base waypoints closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] # Check TL state and adapt velocity of corresponding waypoints # If TL is red --> decelerate # Else --> keep base waypoints 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 generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): #If no traffic light close by within the lookahead limit # just check the max speed and let the car move on lane.waypoints = self.set_waypoints_velocities(base_waypoints, -1) else: lane.waypoints = self.set_waypoints_velocities( base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() closest_wp_index = self.get_closest_waypoint_idx() farthest_wp_index = closest_wp_index + LOOKAHEAD_WPS base_wp = self.base_waypoints.waypoints[ closest_wp_index:farthest_wp_index] # do not modify anything if there's no stop waypoint or the next stop waypoint is # beyond our lookahead window if (self.stop_line_idx < 0) or (self.stop_line_idx > farthest_wp_index): lane.waypoints = base_wp else: lane.waypoints = self.decelerate_waypoints(base_wp, closest_wp_index) return lane
def generate_lane(self): lane = Lane() 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 #lane.waypoints.twist.twist.linear.x *= 1.2 else: #rospy.logwarn("Need to decelerate") lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def publish_waypoints(self): lane = Lane() 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 else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) #lane.waypoints = self.decelerate(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] # Deteremine the waypoints to generate depending on whether stopline waypoints are provided 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 generate_lane(self): lane = Lane() # Compute the closest index to our position closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS # Slice base_waypoints with our closest and farthest indexes base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_id() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints # if (self.stopline_wp_idx >= farthest_idx): # print('too far away') else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) # print("decelerating") return lane
def publish_waypoints(self, closest_idx): # fill in final waypoints to publish lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = self.base_waypoints.waypoints[ closest_idx:closest_idx + LOOKAHEAD_WPS] # fill in path for visulization in Rviz path = Path() path.header.frame_id = '/world' for p in lane.waypoints: path_element = PoseStamped() path_element.pose.position.x = p.pose.pose.position.x path_element.pose.position.y = p.pose.pose.position.y path.poses.append(path_element) self.final_waypoints_pub.publish(lane) self.final_path_pub.publish(path)
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] # rospy.logwarn("farthest {0} and stopline {1}".format(farthest_idx, self.stopline_wp_idx)) if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints else: # rospy.logwarn("decelerate farthest {0} and stopline {1}".format(farthest_idx, self.stopline_wp_idx)) lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_next_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 else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) #rospy.loginfo("lane.waypoints: {}".format(lane.waypoints)) return lane
def generate_lane(state, waypoints, closest_idx, stopline_idx, farthest_idx): lane = Lane() if state is VehicleState.KL: lane.waypoints = waypoints[closest_idx:farthest_idx] elif state is VehicleState.STOP: # We don't really need waypoints further than stopline at this state waypoints = waypoints[closest_idx:stopline_idx] lane.waypoints = WaypointUpdater.decelerate_waypoints( waypoints, closest_idx, stopline_idx) else: raise NotImplementedError() # TODO: ??? why did they removed the header assignment? # lane.header = self.base_waypoints.header return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints # self.red_light_lane_wps = None else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() 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 # print('lane.waypoints:', lane.waypoints) else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) # print('lane.waypoints:', lane.waypoints) return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() # Python is nice with overflow index so we dont have to care about it as # Pythhon will just give the slice from the specified point to the end farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] # If its negative one or further thatn we care about just publish it # otherwise decelerate it 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 generate_lane(self): lane = Lane() lane.header = self.base_lane.header closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] if self.stopline_wp_idx == -1 or self.stopline_wp_idx >= farthest_idx: lane.waypoints = base_waypoints else: rospy.loginfo( 'Decelerating - stopline_wp_idx {}, farthest_idx {}'.format( self.stopline_wp_idx, farthest_idx)) lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() #waypoints(pose, twist) # Get the closest waypoint index closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS # +200 # Cut out waypoints in needed area base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] # If stop line not exist (idx == -1) or stop line exist farther than farthest index, give base_waypoints to lane if (self.stopline_wp_idx == -1) or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints # Else stop line exist in base_waypoints, create waypoints to decelerate and stop before stop line else: lane.waypoints = self.decelerate_waypoints( base_waypoints, closest_idx) # deceleration return lane
def generate_lane(self): lane = Lane() close_idx = self.get_closest_waypoint_idx() far_idx = close_idx + LOOKAHEAD_WPS lane_waypoints = self.base_waypoints.waypoints[close_idx:far_idx] #rospy.loginfo("stop is {a} and far is {b}".format(a=self.stop_idx, b=far_idx)) if self.stop_idx == -1 or (self.stop_idx >= far_idx): rospy.loginfo("Test again: stop is {a} and far is {b}".format( a=self.stop_idx, b=far_idx)) lane.waypoints = lane_waypoints else: rospy.loginfo("Need decel") lane.waypoints = self.decelerate_waypoints(lane_waypoints, close_idx) return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] # rospy.logwarn('closest index: {0}, farthest index: {1}'.format(closest_idx, farthest_idx)) if self.stopline_wp_idx == -1 or (self.stopline_wp_idx >= farthest_idx): lane.waypoints = base_waypoints # rospy.logwarn('use base waypoints') else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) # rospy.logwarn('use decelerate waypoints') return lane
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() lookahead_wps = max(int(self.velocity.twist.linear.x * 4.0), MIN_LOOKAHEAD_WPS) 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 else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def generate_lane(self): lane = Lane() # Get the closest index closest_idx = self.get_closest_waypoint_id() # Set the farthest index farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[ closest_idx:farthest_idx] # If no traffic light was detected, publish the base_waypoints as it is if (self.stop_line_wp_index == -1) or (self.stop_line_wp_index >= farthest_idx): lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate_waypoints(base_waypoints, closest_idx) return lane
def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header # not used # We'll publish at most LOOKAHEAD_WPS waypoints, but as the slicing here implies, # when we get close to the end of the list, the number of waypoints we publish will be # less than LOOKAHEAD_WPS start_idx = closest_idx end_idx = start_idx + LOOKAHEAD_WPS base_waypoints = self.base_waypoints.waypoints[start_idx:end_idx] if self.stopline_wp_idx == -1 or self.stopline_wp_idx >= end_idx: lane.waypoints = base_waypoints else: lane.waypoints = self.decelerate_waypoints(base_waypoints, start_idx) self.final_waypoints_pub.publish(lane)
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] rospy.logwarn( "Current wp idx: {0}, farthest wp idx: {1} stopline idx: {2}". format(closest_idx, farthest_idx, 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.decelerate_waypoints(base_waypoints, closest_idx) return lane
def loop(self): rate = rospy.Rate(20) while not rospy.is_shutdown(): if (self.waypoints and self.next_waypoint_index): lane = Lane() lane.header.frame_id = self.current_pose.header.frame_id lane.header.stamp = rospy.Time(0) lane.waypoints = self.waypoints[self.next_waypoint_index:self.next_waypoint_index+LOOKAHEAD_WPS] if (self.stop_trajectory): start_index = self.stop_trajectory[0] wps = self.stop_trajectory[1] shift = self.next_waypoint_index - start_index for i in range(min(LOOKAHEAD_WPS, len(lane.waypoints))): shifted_i = i + shift lane.waypoints[i] = wps[shifted_i] if (0 <= shifted_i < len(wps)) else lane.waypoints[i] self.final_waypoints_pub.publish(lane) rate.sleep()
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 publish_waypoints(self, final_waypoints): lane = Lane() lane.waypoints = final_waypoints self.final_waypoints_pub.publish(lane)
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)
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(self, waypoints): lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) lane.waypoints = waypoints self.pub.publish(lane)