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)
