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 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 publish_msg(self, final_waypoints): waypoint_msg = Lane() waypoint_msg.header.seq = self.msg_seq waypoint_msg.header.stamp = rospy.Time.now() waypoint_msg.header.frame_id = '/world' waypoint_msg.waypoints = final_waypoints self.final_waypoints_pub.publish(waypoint_msg) self.msg_seq += 1
def generate_lane(self): lane = Lane() closest_idx = self.get_closest_waypoint_idx() farest_idx = closest_idx + LOOKAHEAD_WPS # When nearest light is not red or is farer than LOOKAHEAD_WPS, just ignore if (self.stopline_wp_idx == -1) or (self.stopline_wp_idx >= farest_idx): base_waypoints = self.base_waypoints.waypoints[ closest_idx:farest_idx] lane.waypoints = base_waypoints else: # Need get closest_idx to stopline pose as next waypoints, and need speed down base_waypoints = self.base_waypoints.waypoints[closest_idx:self. stopline_wp_idx] lane.waypoints = self.decelerate(base_waypoints, closest_idx) return lane
def make_lane_msg(self, frame_id, waypoints): """This should be the job of lane constructor """ lane = Lane() lane.header.frame_id = frame_id lane.header.stamp = rospy.Time.now() lane.waypoints = waypoints return lane
def generate_lane(self): # TODO final_lane = Lane() closest_waypoint_index = self.get_closest_points_index() #rospy.logwarn("closest_waypoint_index:{0}".format(closest_waypoint_index)) farthest_waypoint_index = closest_waypoint_index + LOOKAHEAD_WPS base_waypoints_slice = self.base_waypoints.waypoints[closest_waypoint_index:farthest_waypoint_index] if self.stop_line_waypoint_index == -1 or self.stop_line_waypoint_index >= farthest_waypoint_index: final_lane.waypoints = base_waypoints_slice else: #final_lane.waypoints = base_waypoints_slice final_lane.waypoints = self.get_decelerate_waypoint(base_waypoints_slice, closest_waypoint_index) return final_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] for i in range(len(lane.waypoints) - 1): self.set_waypoint_velocity(lane.waypoints, i, TARGET_VEL) self.final_waypoints_pub.publish(lane)
def generate_lane(self): lane = Lane() # Get the closest index closest_idx = self.get_closest_waypoint_idx() # Set the farthest index farthest_idx = closest_idx + LOOKAHEAD_WPS base_waypoints = self.base_lane.waypoints[closest_idx:farthest_idx] # If no traffic light was detected, publish the base_waypoints as it is 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() 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 loop(self): rospy.logdebug ("In WPU loop\n") rate = rospy.Rate(10) # 50 while not rospy.is_shutdown(): # wait for base_waypoints and pose info then if self.base_waypoints is [] or self.car_pose is None: rate.sleep() continue # rospy.logwarn ("In WPU loop: have waypoints and car_pose") # get the index closest waypoint to car index = get_closest_waypoint(self.base_waypoints, self.car_pose) # if index != self.closest_wp_index: # rospy.logwarn("WPULoop - closest_wp_index = %d", index) self.closest_wp_index = index if self.closest_wp_index == -1 : rate.sleep() continue #--------------------------------------------------------------- # otherwise prepare next waypoints_ahead = [] # rospy.logwarn("\n- preparing Lane - 20 waypoints!!!! starting with wp_ix = %d", self.closest_wp_index) for i in range(LOOKAHEAD_WPS): ix = (self.closest_wp_index + i) % len(self.base_waypoints) vel = self.velocity # if we are in the mode of decelerating for a red light if self.vel_incr != 0 : vel = 0 # rospy.logwarn("WPLoop: setting waypoints[%d] = 0", ix) self.set_waypoint_velocity( self.base_waypoints, ix, vel) waypoints_ahead.append(self.base_waypoints[ix]) # structure the data to match the expected styx_msgs/Lane form lane = Lane() lane.waypoints = waypoints_ahead # list of waypoints ahead of the car lane.header.stamp = rospy.Time.now() # timestamp lane.header.frame_id = self.frame_id # publish Lane # rospy.logwarn ("In WPUloop: about to publish lane") self.final_waypoints_pub.publish(lane) rate.sleep()
def pose_cb(self, msg): # TODO: Implement # pos = msg.pose.position # ort = msg.pose.orientation # rospy.loginfo( "[waypoint_updater.pose_cb] position = (%f, %f, %f)", \ # pos.x, pos.y, pos.z ) # rospy.loginfo( "[waypoint_updater.pose_cb] orientation = (%f, %f, %f, %f)", \ # ort.x, ort.y, ort.z, ort.w ) if self.base_wps == None: rospy.loginfo( "[waypoint_updater.pose_cb] No base_waypoints." ) return cpos = msg.pose.position cort = msg.pose.orientation near_i = self.get_closest_waypoint( cpos ) + 1 num_wps = len( self.base_wps.waypoints ) lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) # self.traffic_wp = -1 if self.traffic_wp == -1: if near_i + LOOKAHEAD_WPS > num_wps: lane.waypoints = self.base_wps.waypoints[ near_i : ] + \ self.base_wps.waypoints[ : near_i + LOOKAHEAD_WPS - num_wps ] else: lane.waypoints = self.base_wps.waypoints[ near_i : near_i + LOOKAHEAD_WPS ] elif self.traffic_wp >= near_i: lane.waypoints = self.base_wps.waypoints[ near_i : self.traffic_wp + 1 ] elif near_i - self.traffic_wp <= 10: lane.waypoints = self.base_wps.waypoints[ near_i : near_i + 1 ] else: lane.waypoints = self.base_wps.waypoints[ near_i : ] + \ self.base_wps.waypoints[ : self.traffic_wp + 1 ] # Set velocities if self.traffic_wp == -1: self.accelerate( lane.waypoints, near_i ) else: self.decelerate( lane.waypoints, near_i ) # rospy.loginfo( "[waypoint_updater ===>] car = %d, red = %d", near_i, self.traffic_wp ) self.final_waypoints_pub.publish( 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 publish_nxt_waypoints(self, nxt_idx): waypoints_on_lane = Lane() # copy header waypoints_on_lane.header = self.base_waypoints.header # get all waypoints starting from closest until closest + 200 waypoints_on_lane.waypoints = self.base_waypoints.waypoints[ nxt_idx:nxt_idx + LOOKAHEAD_WPS] # store into final variable self.final_waypoints_pub.publish(waypoints_on_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 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() #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 publish(self, waypoints): rate = rospy.Rate(40) while not rospy.is_shutdown(): lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) lane.waypoints = waypoints self.pub.publish(lane) rate.sleep()
def publish(self, waypoints): if waypoints == []: return lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.get_rostime() lane.waypoints = waypoints self.final_waypoints_pub.publish(lane)
def publish(self, waypoints): lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) lane.waypoints = waypoints self.pub.publish(lane) rate = rospy.Rate(1) # 1hz rate.sleep()
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(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() 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 publish(self, waypoints, base_path): lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) lane.waypoints = waypoints self.pub.publish(lane) self.pub_path.publish(base_path) rospy.loginfo('publishing......\n')
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 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('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 publish_waypoints(self, closest_idx): if not self.stopline_wp_idx: lane = Lane() lane.header = self.base_lane.header lane.waypoints = self.base_lane.waypoints[closest_idx: closest_idx + LOOKAHEAD_WPS] self.final_waypoints_pub.publish(lane) else: final_lane = self.generate_lane() self.final_waypoints_pub.publish(final_lane)
def publish(self, waypoints): """ Publish the given waypoints """ lane = Lane() lane.header.frame_id = self.pose_frame_id lane.header.stamp = rospy.Time.now() lane.waypoints = waypoints self.final_waypoints_pub.publish(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() 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() 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 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 _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 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 publish(self, waypoints): lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) lane.waypoints = waypoints self.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_waypoints(self, final_waypoints): lane = Lane() lane.waypoints = final_waypoints self.final_waypoints_pub.publish(lane)