def publish(self, waypoints):
lane = Lane()
lane.header.frame_id = '/world'
lane.header.stamp = rospy.Time(0)
lane.waypoints = waypoints
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.final_waypoints_pub.publish(lane)
def loop(self):
if self.waypoints != None:
# Find waypoint directly ahead of us and assign it to self.wpt_ahead
self.find_next_waypoint()
# Initialize the final waypoints ahead of us
self.final_wpts = Lane()
if False:
print('WPT Ahead ', "x: ",
self.wpt_ahead.pose.pose.position.x, "y: ",
self.wpt_ahead.pose.pose.position.y, "idx: ",
self.wpt_ahead_idx)
print('WPT List Length: ', len(self.waypoints))
# Form final waypoint list starting from the waypoint directly ahead
final_wpt_idx = self.wpt_ahead_idx + LOOKAHEAD_WPS
if final_wpt_idx < len(
self.waypoints
): # protect against wrapping around the waypoints array
self.final_wpts.waypoints = self.waypoints[
self.wpt_ahead_idx:final_wpt_idx]
else:
# Handle the case when the base waypoint indicies wrap back to zero
self.final_wpts.waypoints = self.waypoints[
self.wpt_ahead_idx:len(self.waypoints)]
idx_prev_0 = len(self.waypoints) - self.wpt_ahead_idx
idx_past_0 = LOOKAHEAD_WPS - idx_prev_0
for idx in range(idx_past_0):
self.final_wpts.waypoints.append(self.waypoints[idx])
# Find the index of the stopline in the final waypoints list
self.num_wpts_to_stopline = self.light_ahead_idx - self.wpt_ahead_idx
if self.num_wpts_to_stopline < 0:
self.num_wpts_to_stopline = 30.
if self.num_wpts_to_stopline == 0:
self.num_wpts_to_stopline = 1.
if self.num_wpts_to_stopline > 30:
self.num_wpts_to_stopline = 30.
idx = 0
fwpt_idx = 0
wpt_ahead = self.final_wpts.waypoints[0]
if self.light_ahead_idx >= 0:
wpt_stopline = self.waypoints[self.light_ahead_idx]
dis_wptahead2stopline = math.sqrt(
(wpt_ahead.pose.pose.position.x -
wpt_stopline.pose.pose.position.x)**2 +
(wpt_ahead.pose.pose.position.y -
wpt_stopline.pose.pose.position.y)**2)
else:
dis_wptahead2stopline = 999.
self.target_speed_mps = self.current_velocity
vel_plan = []
dist_plan = []
for idx, wpt in enumerate(self.final_wpts.waypoints):
# Two Checks before we set deceleration profile:
# 1) Check if we are NOT past the white stop line in the middle of the intersection => dont stop in middle
# 2) Check if we close enough to the stop line to start braking
# If either are false then just continue at normal speed
#in_intersection = dis_wptahead2stopline < 3. # arbitrary tuned number
start_braking = math.fabs(
dis_wptahead2stopline) < STOP_DIST_ERR
if start_braking: #and not in_intersection:
# For each waypoint compute in order:
# Distance to the stoplight from waypoint
# Distance from waypoint to stopline in front of stoplight
# Desired deceleration
# Final velocity command for waypoint
# Find the distance from the final wpt to the stoplight
dis_wpt2stopline = math.sqrt(
(wpt.pose.pose.position.x -
wpt_stopline.pose.pose.position.x)**2 +
(wpt.pose.pose.position.y -
wpt_stopline.pose.pose.position.y)**2)
# Find the desired final stop position of the car
self.target_speed_mps = self.target_speed_mps - 2.0 * 1. / self.num_wpts_to_stopline * dis_wpt2stopline
if self.target_speed_mps < 3.0: # if the velocity is small, just command vel to 0
self.target_speed_mps = 0.0
if idx >= self.num_wpts_to_stopline: # if the final wpts are ahead of the stopline set the commanded vel to 0
self.target_speed_mps = 0.0
if idx < self.num_wpts_to_stopline and dis_wpt2stopline < 2.: # if the stopline is 2m ahead just stop
self.target_speed_mps = 0.0
braking = self.current_velocity > 1e-2
else:
# Car goes at normal speed
self.target_speed_mps = self.base_wpt_spd_mps #SPEED_MPH*MPH2MPS
braking = False
dis_wpt2stopline = 999.
# Set speeds in waypoint list
wpt.twist.twist.linear.x = self.target_speed_mps
vel_plan.append(self.target_speed_mps) # for debugging
dist_plan.append(dis_wpt2stopline)
#Publish final waypoints
self.final_wpts.header.stamp = rospy.Time.now()
self.final_waypoints_pub.publish(self.final_wpts)
if DEBUG_TGTSPD and braking:
#print('Distance Plan: {0:.1f}--{1:.1f}--{2:.1f}--{3:.1f}--{4:.1f}--{5:.1f}--{6:.1f}--{7:.1f}--{8:.1f}--{9:.1f}--{10:.1f}--{11:.1f}--{12:.1f}--{13:.1f}--{14:.1f}--{15:.1f}--{16:.1f}--{17:.1f}--{18:.1f}--{19:.1f}--{20:.1f}--{21:.1f}--{22:.1f}--{23:.1f}--{24:.1f}--{25:.1f}--{26:.1f}--{27:.1f}--{28:.1f}--{29:.1f}'.format(\
#dist_plan[0], dist_plan[1],dist_plan[2],dist_plan[3],dist_plan[4],dist_plan[5],dist_plan[6],dist_plan[7],dist_plan[8],dist_plan[9],dist_plan[10],\
#dist_plan[11],dist_plan[12],dist_plan[13],dist_plan[14],dist_plan[15],dist_plan[16],dist_plan[17],dist_plan[18],dist_plan[19],dist_plan[20],\
#dist_plan[21],dist_plan[22],dist_plan[23],dist_plan[24],dist_plan[25],dist_plan[26],dist_plan[27],dist_plan[28],dist_plan[29]))
print('Velocity Plan: {0:.1f}--{1:.1f}--{2:.1f}--{3:.1f}--{4:.1f}--{5:.1f}--{6:.1f}--{7:.1f}--{8:.1f}--{9:.1f}--{10:.1f}--{11:.1f}--{12:.1f}--{13:.1f}--{14:.1f}--{15:.1f}--{16:.1f}--{17:.1f}--{18:.1f}--{19:.1f}--{20:.1f}--{21:.1f}--{22:.1f}--{23:.1f}--{24:.1f}--{25:.1f}--{26:.1f}--{27:.1f}--{28:.1f}--{29:.1f}'.format(\
vel_plan[0],vel_plan[1],vel_plan[2],vel_plan[3],vel_plan[4],vel_plan[5],vel_plan[6],vel_plan[7],vel_plan[8],vel_plan[9],vel_plan[10],\
vel_plan[11],vel_plan[12],vel_plan[13],vel_plan[14],vel_plan[15],vel_plan[16],vel_plan[17],vel_plan[18],vel_plan[19],vel_plan[20],\
vel_plan[21],vel_plan[22],vel_plan[23],vel_plan[24],vel_plan[25],vel_plan[26],vel_plan[27],vel_plan[28],vel_plan[29]))
#Topics to publish for debugging
if DEBUG_TOPICS:
self.debug_currpos = PoseStamped()
self.debug_currpos.header.stamp = rospy.Time.now()
self.debug_currpos.pose.position.x = self.pos_x
self.debug_currpos.pose.position.y = self.pos_y
self.debug_currpos.pose.position.z = self.pos_z
self.debug_currpos.pose.orientation = self.current_orient
self.debug_currentpos_pub.publish(self.debug_currpos)
pass
def loop(self):
rate = rospy.Rate(5) # 5Hz
while not rospy.is_shutdown():
if self.pose is not None and self.waypoints is not None and self.traffic is not None and self.current_velocity is not None:
ego_theta = 2.*math.acos(self.pose.orientation.w)
ego_vx = np.dot(np.array([[math.cos(ego_theta),-math.sin(ego_theta)],
[math.sin(ego_theta), math.cos(ego_theta)]]), np.array([1,0]))
ego_vy = np.dot(np.array([[0,-1],[1,0]]), ego_vx)
#rospy.logerr(ego_theta)
# publish final_waypoints
min_distance = 1e9
initial_wp_index = 0
for k in xrange(len(self.waypoints)):
i = k
if self.previous_initial_wp_index is not None:
i = (i + self.previous_initial_wp_index) % len(self.waypoints)
waypoint = self.waypoints[i].pose.pose.position
np_waypoint = np.array([waypoint.x,waypoint.y])
np_ego_pose = np.array([self.pose.position.x,self.pose.position.y])
np_diff = np_waypoint - np_ego_pose
distance = np.linalg.norm(np_diff)
param = np.dot(np.linalg.inv(np.vstack((ego_vx,ego_vy)).transpose()), np_diff)
# If the waypoint is in front of vehicle and also the closest one,
# update the index.
if(param[0] > 0 and distance < min_distance):
min_distance = distance
initial_wp_index = i % len(self.waypoints)
if self.previous_initial_wp_index is not None:
break
# publish
final_waypoints = Lane()
final_waypoints.header = std_msgs.msg.Header()
final_waypoints.header.stamp = rospy.Time.now()
actual_speed = self.current_velocity.linear.x
for i in xrange(LOOKAHEAD_WPS):
index = (initial_wp_index + i) % len(self.waypoints)
final_waypoints.waypoints.append(self.waypoints[index])
self.previous_initial_wp_index = initial_wp_index
v_limit = rospy.get_param('/waypoint_loader/velocity') / 3.6 # Speed limit given by ROS parameter
v_limit *= 0.9 # Set margin to not exceed speed limit.
v0 = min(20./2.24, v_limit) # This program allows maximum spped of 20mph.
if self.traffic == -1:
for i in xrange(LOOKAHEAD_WPS):
self.set_waypoint_velocity(final_waypoints.waypoints, i, v0)
else:
# t0
# v0 ----------------
# \
# \
# \ a0
# \
# \
# \___________ v=0
# t1
# target velocity diagram
#
a0 = 2.5 # m/s^2 target acceleration
margin = 10 # m target margin before stop line
r0 = self.distance(self.waypoints,initial_wp_index,self.traffic) - margin # target position to stop
stop_space = v0*v0/(2*a0)
s_start_brake = r0 - stop_space
#t1 = 0.5*(2.*r0/v0 + v0/a0)
#t0 = 0.5*(2.*r0/v0 - v0/a0)
#rospy.loginfo("DBG STOP_S: {0} BRAKE_S {1} TOTAL {2}".format(stop_space,s_start_brake,r0))
for i in xrange(LOOKAHEAD_WPS):
r = self.distance(self.waypoints,initial_wp_index,initial_wp_index+i)
if r <= s_start_brake: #v0 * t0:
v = v0
elif s_start_brake < r and r < r0:
ds = r-s_start_brake
#rospy.loginfo("DBG DS: {0} v0 {1} ".format(ds,v0))
delta = v0*v0 - 2*a0*ds
t = (v0 - math.sqrt(delta))/a0
v = v0 - a0*t
#v = math.sqrt(2.*a0*v0*t0 + v0*v0 - 2.*a0*r)
else:
v = 0
#rospy.loginfo("SPEED: {0}".format(v))
self.set_waypoint_velocity(final_waypoints.waypoints, i, v)
#rospy.loginfo("SPEED: -------------")
self.final_waypoints_pub.publish(final_waypoints)
rate.sleep()
def generate_final_waypoints(self):
select_wps = []
start_idx = -1
end_idx = -1
# waypoints from base waypoints
if self.waypoints:
# waypoint closest to car
start_idx = self.get_closest_waypoint(self.curr_pose)
# slice LOOKAHEAD_WPS number of waypoints from base waypoints
end_idx = start_idx + LOOKAHEAD_WPS
if end_idx > self.waypoints_count:
select_wps = self.waypoints[start_idx:] + self.waypoints[0:end_idx % self.waypoints_count]
else:
select_wps = self.waypoints[start_idx:end_idx]
# rospy.logwarn('final waypoints range: (' + str(start_idx) + ',' + str(end_idx % self.waypoints_count) + ')')
final_wps = Lane()
final_wps.waypoints = select_wps
# if upcoming red light -> Decelerate (to full stop)
# else -> Accelerate (to max velocity)
# Deceleration
if self.stop_idx != -1:
for i in range(len(select_wps)):
self.set_waypoint_velocity(select_wps, i, 0.0)
stop_idx_in_select_wps = self.stop_idx - start_idx
# Using sudden stop mechnism. It is just a step responce and performs quite well
# for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*1.5)):
# self.set_waypoint_velocity(select_wps, i, MAX_SPEED)
for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*0.5)):
self.set_waypoint_velocity(select_wps, i, MAX_SPEED/10)
for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*1.0)):
self.set_waypoint_velocity(select_wps, i, MAX_SPEED/2)
for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*3.0)):
self.set_waypoint_velocity(select_wps, i, MAX_SPEED)
# Acceleration
else:
# Acceleration should be gradual. Although throttle=1.0 works OK in the simulator
# it would be rough for Carla
# We can use increments in steps as used for deceleration
for i in range(len(select_wps)):
self.set_waypoint_velocity(select_wps, i, MAX_SPEED) # This should be smaller than 10
if DEBUG:
v = []
for w in select_wps:
v.append(w.twist.twist.linear.x)
rospy.logwarn('Velocities of selected waypoints:')
rospy.logwarn('{}'.format(v))
return final_wps
def publish_waypoints(self, final_waypoints):
lane = Lane()
lane.header = self.base_waypoints.header
lane.waypoints = final_waypoints
self.final_waypoints_pub.publish(lane)
def publish_waypoints(self):
lane = Lane()
final_lane = self.generate_lane()
self.final_waypoints_pub.publish(final_lane)
def process_waypoints(self, event):
if not self.pose:
rospy.logwarn('no pose has been set')
return None
if not self.current_velocity:
rospy.logwarn('no velocity has been set')
return None
if not self.waypoints:
rospy.logwarn('no waypoints has been set')
return None
# get nearest waypoint
begin = self.get_nearest_waypoint()
end = begin + LOOKAHEAD_WPS
end = min(end, len(self.waypoints))
rospy.loginfo("begin {}, end {}, waypoints len {}".format(
begin, end, len(self.waypoints)))
waypointsArr = []
epsilon = 1.0
distance_to_red_light = 0
distance_to_stop = 0
velocity = 0
previous_target_velocity = 0
for i in range(begin, end):
target_velocity = self.max_velocity
if self.red_light_waypoint > 0:
distance_to_red_light = self.distance(self.waypoints, i,
self.red_light_waypoint)
distance_to_stop = max(
0, distance_to_red_light -
self.stop_distance_to_traffic_light)
velocity = self.get_waypoint_velocity(self.waypoints[i])
# slow down getting closer to intersection
if distance_to_stop > 0 and distance_to_stop < self.decelerating_distance:
target_velocity *= distance_to_red_light / self.decelerating_distance
# push down the brakes a bit harder on the second half
if distance_to_stop > 0 and distance_to_stop < self.decelerating_distance * 0.5:
target_velocity *= distance_to_stop / self.decelerating_distance
# keep a minimum target velocity
target_velocity = max(2.0, target_velocity)
# perform the brake motion
if distance_to_stop > 0.5 and distance_to_stop <= 2:
target_velocity = 1.0
if distance_to_stop > 0 and distance_to_stop <= 0.5:
target_velocity = 0.33
elif distance_to_stop == 0:
target_velocity = 0.0
# accelerate smoothly
start_waypoint_velocity = self.get_waypoint_velocity(
self.waypoints[begin])
previous_target_velocity = start_waypoint_velocity if i == begin else previous_target_velocity
curr_waypoint_velocity = self.get_waypoint_velocity(
self.waypoints[i])
rospy.logdebug(
"{} = start v {}, current v {}, prev v {}, target v {}".format(
i, start_waypoint_velocity, curr_waypoint_velocity,
previous_target_velocity, target_velocity))
if start_waypoint_velocity == 0 and curr_waypoint_velocity == 0 and previous_target_velocity == 0:
target_velocity = (0.25 * target_velocity +
0.75 * previous_target_velocity)
elif previous_target_velocity < target_velocity:
target_velocity = (0.1 * target_velocity +
0.9 * previous_target_velocity)
# make sure target_velocity doesn't suprass max allowed velocity
if target_velocity > 4.0:
target_velocity = self.max_velocity
# make sure target velocity is within bounds
target_velocity = min(max(0, target_velocity), self.max_velocity)
# store previous target velocity
previous_target_velocity = target_velocity
rospy.loginfo(
"waypoint {}, decelerating distance {}, distance to red light {}, "
"distance to stop {}, velocity {}, target velocity {}".format(
i, self.decelerating_distance, distance_to_red_light,
distance_to_stop, velocity, target_velocity))
self.set_waypoint_velocity(self.waypoints, i, target_velocity)
waypointsArr.append(self.waypoints[i])
lane = Lane()
lane.waypoints = waypointsArr
self.final_waypoints_pub.publish(lane)
def update_waypoints(self):
if self.current_pose is None or self.base_waypoints is None:
return
closest_waypoint_index = self.get_closest_waypoint_index()
target_vel = self.get_waypoint_velocity(
self.base_waypoints[closest_waypoint_index])
distance_to_light = None
output_waypoints = []
curr_traffic_light = self.trafficlight
if curr_traffic_light is not None and closest_waypoint_index is not None:
distance_to_light = self.distance(self.base_waypoints,
closest_waypoint_index,
curr_traffic_light)
if distance_to_light is not None and distance_to_light < TRAFFIC_LIGHT_RANGE:
velocity = target_vel * distance_to_light / TRAFFIC_LIGHT_RANGE
if velocity < 1:
velocity = 0
if curr_traffic_light - closest_waypoint_index > 2:
output_waypoints.append(
self.clone_waypoint(
self.base_waypoints[closest_waypoint_index]))
self.set_waypoint_velocity(output_waypoints, -1, velocity)
for i in range(closest_waypoint_index + 1,
curr_traffic_light - 2):
waypoint_index = i % len(self.base_waypoints)
segment_distance = self.distance(self.base_waypoints,
waypoint_index - 1,
waypoint_index)
decel_velocity = (segment_distance / distance_to_light
) * self.current_velocity
velocity -= decel_velocity
if velocity < 1:
velocity = 0
output_waypoints.append(
self.clone_waypoint(
self.base_waypoints[waypoint_index]))
self.set_waypoint_velocity(output_waypoints, -1, velocity)
if curr_traffic_light == closest_waypoint_index:
output_waypoints.append(
self.clone_waypoint(
self.base_waypoints[curr_traffic_light]))
self.set_waypoint_velocity(output_waypoints, -1, 0)
else:
output_waypoints.append(
self.clone_waypoint(
self.base_waypoints[curr_traffic_light - 1]))
self.set_waypoint_velocity(output_waypoints, -1, 0)
output_waypoints.append(
self.clone_waypoint(
self.base_waypoints[curr_traffic_light]))
self.set_waypoint_velocity(output_waypoints, -1, 0)
if len(output_waypoints) < LOOKAHEAD_WPS:
remaining_wps = LOOKAHEAD_WPS - len(output_waypoints)
for i in range(curr_traffic_light,
curr_traffic_light + remaining_wps):
waypoint_index = i % len(self.base_waypoints)
output_waypoints.append(
self.clone_waypoint(
self.base_waypoints[waypoint_index]))
self.set_waypoint_velocity(output_waypoints, -1, 0)
else:
for i in range(closest_waypoint_index,
closest_waypoint_index + LOOKAHEAD_WPS):
waypoint_index = i % len(self.base_waypoints)
output_waypoints.append(
self.clone_waypoint(self.base_waypoints[waypoint_index]))
self.set_waypoint_velocity(output_waypoints, -1, target_vel)
lane = Lane()
self.final_waypoints = output_waypoints
lane.waypoints = output_waypoints
rospy.loginfo(rospy.get_name() + ': waypoints published')
self.final_waypoints_pub.publish(lane)
def get_lane_object(id, waypoints):
lane = Lane()
lane.header.frame_id = id
lane.header.stamp = rospy.Time.now()
lane.waypoints = waypoints
return lane
def publish_waypoints(self):
# TODO : This function publishes the next waypoints
if self.received_waypoints and self.load_current_pose:
next_wp_idx = self.get_next_waypoint()
#rospy.loginfo("next_wp_idx = %s"% (next_wp_idx))
array_final_waypoints = Lane()
red_light_idx_in_final_waypoints = None
for idx_waypt in range(LOOKAHEAD_WPS):
# Check if drive more than one round
idx_waypt_to_append = (next_wp_idx + idx_waypt) % len(
self.base_waypoints.waypoints)
#Get the information about the waypoint that we will append
waypt_to_append = self.base_waypoints.waypoints[
idx_waypt_to_append]
# print "TARGET SPEED for each point:", self.get_waypoint_velocity(waypt_to_append)
# Append the waypoint to the array of waypoints.
array_final_waypoints.waypoints.append(waypt_to_append)
#rospy.loginfo("self.cur_red_light_wp_idx = %s, idx_waypt_to_append = %s"% (self.cur_red_light_wp_idx, idx_waypt_to_append))
if (self.cur_red_light_wp_idx and
(self.cur_red_light_wp_idx == idx_waypt_to_append)):
red_light_idx_in_final_waypoints = idx_waypt
#for i in range(5):
# print "-------Index:", i," ;target velocity:", self.get_waypoint_velocity(array_final_waypoints.waypoints[i])
#Once the final waypoints is built, check for red-lights and reduce the velocieties of way points to gracefully halt the vehicle at red light
if (red_light_idx_in_final_waypoints):
total_dist_to_red_light = self.distance(
array_final_waypoints.waypoints, 0,
red_light_idx_in_final_waypoints)
rospy.loginfo("Number of wps to stopline %s" %
(red_light_idx_in_final_waypoints))
for i in range(len(array_final_waypoints.waypoints)):
if (i < red_light_idx_in_final_waypoints - 13):
#reduce the velocity
dist = self.distance(array_final_waypoints.waypoints,
i,
red_light_idx_in_final_waypoints
) #distance to red light
decel = 1 # decelerate at 1 m2/s2
vel = math.sqrt(
2 * decel * dist
) #based on distance, calcualte velocity and set it in waypoint
if (vel < self.get_waypoint_velocity(
array_final_waypoints.waypoints[i])):
self.set_waypoint_velocity(
array_final_waypoints.waypoints, i,
vel) #set velocity
else:
self.set_waypoint_velocity(
array_final_waypoints.waypoints, i, 0)
else:
for i in range(len(array_final_waypoints.waypoints)):
self.set_waypoint_velocity(array_final_waypoints.waypoints,
i, self.speed_limit)
# for i in range(5):
# print "-------Index:", i," ;target velocity:", self.get_waypoint_velocity(array_final_waypoints.waypoints[i])
#TEST START
#if(red_light_idx_in_final_waypoints == 1):
# self.cur_red_light_wp_idx = (next_wp_idx + 200) % len(self.base_waypoints.waypoints)
#TEST END
# Publish the Lane info to the /final_waypoints topic
self.final_waypoints_pub.publish(array_final_waypoints)
def loop(self):
rate = rospy.Rate(self.loop_frequency)
while not rospy.is_shutdown():
# Stay still until traffic lights can be detected
if not self.tl_detector_initialized:
rospy.logwarn(
'Waypoint updater holding until traffic light detector is initialized'
)
rate.sleep()
continue
if self.current_pose != None and self.base_waypoints != None:
xyz_position = self.current_pose.position
quaternion_orientation = self.current_pose.orientation
p = xyz_position
qo = quaternion_orientation
p_list = [p.x, p.y, p.z]
qo_list = [qo.x, qo.y, qo.z, qo.w]
euler = euler_from_quaternion(qo_list)
yaw_rad = euler[2]
closest_waypoint_idx = None
closest_waypoint_dist = None
for idx in range(len(self.base_waypoints)):
wcx, wcy = self.get_on_car_waypoint_x_y(p, yaw_rad, idx)
if closest_waypoint_idx is None:
closest_waypoint_idx = idx
closest_waypoint_dist = math.sqrt(wcx**2 + wcy**2)
else:
curr_waypoint_dist = math.sqrt(wcx**2 + wcy**2)
if curr_waypoint_dist < closest_waypoint_dist: #
closest_waypoint_idx = idx
closest_waypoint_dist = curr_waypoint_dist
wcx, wcy = self.get_on_car_waypoint_x_y(
p, yaw_rad, closest_waypoint_idx)
while wcx < 0.:
closest_waypoint_idx = (closest_waypoint_idx +
1) % self.base_waypoints_count
wcx, wcy = self.get_on_car_waypoint_x_y(
p, yaw_rad, closest_waypoint_idx)
next_waypoints = []
for loop_idx in range(LOOKAHEAD_WPS):
wp_idx = (loop_idx +
closest_waypoint_idx) % self.base_waypoints_count
next_waypoints.append(self.get_waypoint_to_sent(wp_idx))
rospy.loginfo('INDEX {} wc [{:.3f},{:.3f}]'.format(
closest_waypoint_idx, wcx, wcy))
lane = Lane()
lane.header.frame_id = '/world'
lane.header.stamp = rospy.Time(0)
lane.waypoints = self.adjust_velocity_to_stop(
next_waypoints, closest_waypoint_idx)
self.final_waypoints_pub.publish(lane)
rate.sleep()
