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)
