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)
