def decelerate_waypoints(self, waypoints, closest_idx):
temp = []
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
stop_idx = max(self.stopline_wp_idx - closest_idx - 2, 0)
dist = self.distance(waypoints, i, stop_idx)
vel = math.sqrt(2 * MAX_DECEL * dist)
if vel < 1.:
vel = 0.
p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
temp.append(p)
return temp
def decelerate_waypoints(self,waypoints,closest_idx): # deceleration logic temp=[] for i,wp in enumerate(waypoints): p=Waypoint() #instantiate a waypoint object p.pose=wp.pose stop_idx=max(self.stopline_wp_idx - closest_idx -2, 0) # subtract two way-points to ensure nose of car stops at line dist=self.distance(waypoints,i,stop_idx) # calculate distance from current index to stop line index vel=math.sqrt(2.0*MAX_DECEL*dist) # some deceleration rate based on distance if vel<1.0: vel=0.0 p.twist.twist.linear.x=min(vel,wp.twist.twist.linear.x) #set way point velocity to min of calculated vs speed limit (default value) temp.append(p) return temp
def _decelerate_waypoints(self, base_waypoints, closest_idx):
temp = []
for i, wp in enumerate(base_waypoints):
p = Waypoint()
p.pose = wp.pose
# Two base_waypoints back from line so front of car stops at line
# (-2 is from the center of the car)
stop_idx = max(self.stopline_wp_idx - closest_idx - 2, 0)
dist = self.distance(base_waypoints, i, stop_idx)
# TODO: consider a different function than sqare root
vel = np.power(MAX_DECEL * dist * 7, 0.33) * 1.7
if vel < 1.0:
vel = 0
p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
temp.append(p)
return temp
def set_velocity(waypoints, start_index, wp_index_to_stop): #[Waypoint]
# rospy.loginfo("start_index: %d, wp_index_to_stop: %d" % (start_index, wp_index_to_stop))
if wp_index_to_stop == -1 or wp_index_to_stop >= start_index + len(
waypoints):
# use the original velocity of the base waypoints
return waypoints
wps_with_vel = []
for wp_i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
stop_wp_index = max(wp_index_to_stop - start_index - 2, 0)
dist_to_stop = WaypointUpdater.distance(waypoints, wp_i,
stop_wp_index)
vel = math.sqrt(2. * MAX_DECEL * dist_to_stop)
if vel < 1.:
vel = 0.
p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
wps_with_vel.append(p)
return wps_with_vel
def decelerate_waypoints(self, waypoints, closest_idx):
temp = []
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
# Two waypoints back from the line so front of the car stops at line
stop_idx = max(self.stopline_wp_index - closest_idx - 2, 0)
# Decelerate linearly
# velocity = dist / 5
# Decelerate by S-curve
# velocity = wp.twist.twist.linear.x * (1. - 1. / (1. + math.exp(-(i - stop_idx + 30.) / 6.)))
dist = self.distance(waypoints, i, stop_idx)
velocity = math.sqrt(2 * MAX_DECEL * dist)
if velocity < 1.:
velocity = 0.
p.twist.twist.linear.x = min(velocity, wp.twist.twist.linear.x)
temp.append(p)
return temp
def decelerate_waypoints(self,waypoints,closest_idx):
temp = []
stop_idx = max(self.next_light_wp - closest_idx -5,0)
#rospy.loginfo("stop index is %s",stop_idx)
#waypoints = self.set_velocity(waypoints,0.)
#rospy.loginfo("i is %s", i)
#rospy.loginfo("stop_idx is %s", stop_idx)
#stop index reached. stop the vehicle.
for i , wp in enumerate(waypoints):
#waypoint_till_traffic_light = stop_idx - i
#rospy.loginfo("waypoint till traffic is %s", waypoint_till_traffic_light)
#rospy.loginfo(" i is %s" , i)
p = Waypoint()
p.pose = wp.pose
dist = self.distance(waypoints,i,stop_idx)
#rospy.loginfo("i is less than stop index")
#rospy.loginfo("dist is %s",dist)
vel = K_CONSTANT * dist
if vel < 1.:
vel = 0.
p.twist.twist.linear.x = min(vel,wp.twist.twist.linear.x)
#rospy.loginfo("final velocity is %s", p.twist.twist.linear.x)
temp.append(p)
return temp
def process(self):
# TODO: Use self.final_waypoints_pub to publish the next target waypoints
# In phase 1: we can ignore traffic lights and simply output the next N waypoints *ahead of the car*, with their default velocity
# In phase 2: you need to adjust target speeds on waypoints in order to smoothly brake until the car reaches the waypoint
# corresponding to the next red light's stop line (stored in self.next_traffic_light_stopline_index, == -1 if no next traffic light).
# Advice: make sure to complete dbw_node and have the car driving correctly while ignoring traffic lights before you tackle phase 2
msg = Lane()
closest_idx = self.waypoints_db.get_next_closest_idx(
self.current_car_position)
farthest_idx = closest_idx + self.N
base_waypoints = self.waypoints_db.waypoints[closest_idx:farthest_idx]
if self.next_traffic_light_stopline_index == -1 or (
self.next_traffic_light_stopline_index >= farthest_idx):
msg.waypoints = base_waypoints
else:
new_points = []
for i, wp in enumerate(base_waypoints):
point = Waypoint()
point.pose = wp.pose
stop_idx = max(
self.next_traffic_light_stopline_index - closest_idx - 4,
0)
dist = self.waypoints_db.distance(base_waypoints, i, stop_idx)
vel = math.sqrt(2 * self.max_deceleration * 0.90 * dist)
if vel < 1.0:
vel = 0.0
point.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
new_points.append(point)
msg.waypoints = new_points
self.final_waypoints_publisher.publish(msg)
def decelerate_waypoints(self, base_wp, closest_idx):
# use a temporary list to NOT modify the real base waypoints (only published once)
temp_list = []
# stop some waypoints in front of the stopline to avoid the car's nose to go over
stop_idx = max(self.stop_line_idx - closest_idx - 4, 0)
# for each of the waypoints between us and the stopline, set the velocity
for i, wp in enumerate(base_wp):
p = Waypoint()
p.pose = wp.pose
distance = self.distance(base_wp, i, stop_idx)
MAX_DECEL = 1
vel = math.sqrt(2 * MAX_DECEL * distance)
if vel < 1.:
vel = 0.
# the current twist value of the base waypoint is the speed limit -- no speeding here!
p.twist.twist.linear.x = min(vel, base_wp[i].twist.twist.linear.x)
temp_list.append(p)
return temp_list
def accelerate_waypoints(self, waypoints):
temp = []
last_vel = self.current_vel
last_time_to_next_wp = 0.0
#rospy.logwarn("Waypoint length: {0}".format(len(waypoints)))
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
time_to_next_wp = 0.0
dist = 0.0
# To calculate accel to be added, it is necessary to determine the time it will
# take at the current velocity to reach the next waypoint
if i == len(waypoints) - 1:
new_vel = last_vel + last_time_to_next_wp * self.max_accel
else:
dist = self.distance(waypoints, i, i + 1)
if last_vel < 1.0:
new_vel = 1.0
else:
time_to_next_wp = dist / last_vel
new_vel = last_vel + (self.max_accel * time_to_next_wp)
#rospy.logwarn("Last velocity: {0}, New velocity: {1}, TTNWP: {2}, Dist: {3}".format(last_vel, new_vel, time_to_next_wp, dist))
new_vel = min(new_vel, wp.twist.twist.linear.x)
last_vel = new_vel
last_time_to_next_wp = time_to_next_wp
p.twist.twist.linear.x = new_vel
temp.append(p)
return temp
def decelerate_waypoints(self, base_waypoints, closest_idx):
temp = []
for i, wp in enumerate(base_waypoints):
p = Waypoint()
p.pose = wp.pose
stop_idx = max(self.stopline_wp_idx - closest_idx - 5,
0) # Car is 5 wp long
dist = self.distance(base_waypoints, i, stop_idx)
# https://physics.stackexchange.com/questions/3818/stopping-distance-frictionless
vel = math.sqrt(2 * MAX_DECEL *
dist) # velocity decreases as dist decreases
if vel < 1.:
vel = 0.
p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
temp.append(p)
return temp
def decelerate_waypoints(self, waypoints, closest_idx):
temp = []
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
# Distance includes a number of waypoints back so front of car stops at line
stop_idx = max(self.stopline_wp_idx - closest_idx - STOP_LINE_MARGIN, 0)
dist = self.distance(waypoints, i, stop_idx)
vel = math.sqrt(2 * 0.5 * dist) + (i * DECEL)
if vel < 1.0:
vel = 0.0
p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
temp.append(p)
self.decelerate_count += 1
if (self.decelerate_count % THROTTLE_NO) == 0:
size = len(waypoints) - 1
vel_start = temp[0].twist.twist.linear.x
vel_end = temp[size].twist.twist.linear.x
rospy.logwarn("DECEL: vel[0]={:.2f}, vel[{}]={:.2f}".format(vel_start, size, vel_end))
return temp
def decelerate_waypoints(self, waypoints, stop_idx):
waypoints_up_to_stop = waypoints[:stop_idx]
dists_to_stop = self.distances_to_end(waypoints_up_to_stop)
result = []
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
if i >= stop_idx:
vel = 0
else:
dist_to_stop = dists_to_stop[i]
# Linear decrease in velocity wrt time. We could smoothen this.
vel = math.sqrt(2 * MAX_DECEL * dist_to_stop)
# obey speed limit
vel = min(vel, wp.twist.twist.linear.x)
p.twist.twist.linear.x = vel
result.append(p)
return result
def decelerate_waypoints(self, waypoints, closest_idx):
temp = []
stop_idx = max(
self.stopline_wp_idx - closest_idx - 4, 0
) # Four waypoints back from stop line so front of car stops before stop line
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
braking_vel = 0.0
dist = self.distance(waypoints, i, stop_idx)
if dist > 0.0:
time_to_complete_stop = math.sqrt(dist * 2.0 / MAX_DECEL)
braking_vel = 2.0 * dist / time_to_complete_stop
if braking_vel < 0.1:
braking_vel = 0.0
p.twist.twist.linear.x = min(braking_vel, wp.twist.twist.linear.x)
temp.append(p)
return temp
def decelerate_waypoints(self, waypoints, closest_index):
"""
Function : Decelerate the car if red light and stop close to it
Input Args : Waypoints and closest Red light index
Output : temporary deceleration waypoints
"""
rospy.logerr("In Decelerate waypoints")
temp_waypoints = []
print("Waypoints : ", waypoints)
print("Closest WP index : ", closest_index)
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
stop_index = max(
self.red_trafficlight_waypoint - closest_index - 3, 0)
distance = self.distance(waypoints, i, stop_index)
velocity = math.sqrt(
2 * DECELARATION_LIMIT * distance
) #+ (i * SMOOTH_DECEL) #SMOOTH_DECEL needed for smoother slowing
# Can also implement a S curve implementation here
if velocity < 1.0:
velocity = 0.
p.twist.twist.linear.x = min(velocity, wp.twist.twist.linear.x)
temp_waypoints.append(p)
self.decelerate_count += 1
if (self.decelerate_count % 100) == 0.0:
wp_size = len(waypoints) - 1
velocity_start = temp_waypoints[0].twist.twist.linear.x
velocity_end = temp_waypoints[wp_size].twist.twist.linear.x
# Logging the messages for information and documentation
return temp_waypoints
def publish(self):
wps_pub = Lane()
self.find_next_waypoint()
#determine the list of way points to publish
distance_tl = None
state = "run"
#check red light distance
#TODO will it detect yellow light?
rospy.logwarn("Index: waypoint, redlight {}, {}".format(self.next_waypoint_index,self.red_light_index))
if self.red_light_index and self.next_waypoint_index and self.red_light_index > self.next_waypoint_index:
#distance to the next traffic light in meters
distance_tl = self.distance(self.base_waypoints, self.next_waypoint_index, self.red_light_index)
distance_tl = distance_tl - DIST_LIGHT_LINE #minus the distance from stop line to the traffic light
rospy.logwarn("Car is {:.2f} meters from the stopping light".format(distance_tl))
#check car's current speed
if self.twist:
self.current_velocity = self.twist.twist.linear.x #meters per second
self.current_velocity_mph = 2.23694 * self.current_velocity # miles per hour
#rospy.logwarn("Current speed is {:.2f} MPH".format(self.current_velocity_mph))
#Stop the car if it cannot pass the line within 1.5 seconds. safe to brake
if distance_tl and self.current_velocity > 0 and distance_tl / self.current_velocity > 1 and distance_tl < BRAKE_DIS:
rospy.logwarn("Time to pass the stop line: {}".format(distance_tl / self.current_velocity))
state = "brake"
else: #if it's green light ahead, or safe to pass through the light
state = "run"
max_speed = self.max_velocity_km*0.2778*0.9 #max speed in meter per second
rospy.logwarn("Cat state is set to be {}".format(state))
if state == 'brake':
index_stop_line = self.red_light_index - WP_GAP_LINE_LIGHT
num_wp_stopping = index_stop_line - self.next_waypoint_index #number of ways points used to stop the car
#linearly decelerate the car
des = self.current_velocity / (num_wp_stopping +0.001)
if self.next_waypoint_index is not None:
wp_index = self.next_waypoint_index
for i in range(LOOKAHEAD_WPS):
base_wp = self.base_waypoints[wp_index]
next_wp = Waypoint()
next_wp.pose = base_wp.pose #position
next_wp.twist = base_wp.twist #Speed
#if the car shall brake, and setting reduced speed for all points leading to the stop line.
if state == 'brake' and i <= num_wp_stopping:
if i == num_wp_stopping:
next_wp.twist.twist.linear.x = 0 # the way point that the car shall stop
else:
next_wp.twist.twist.linear.x = (self.current_velocity - des * (i+1))
else:
next_wp.twist.twist.linear.x = max_speed#convert miles per hour to meters per second
wps_pub.waypoints.append(next_wp)
wp_index = (wp_index+1) % self.num_waypoints
#rospy.logwarn('Next way point published:{}'.format(wps_pub))
#rospy.logwarn('way points published:{}'.format(len(wps_pub.waypoints)))
self.final_waypoints_pub.publish(wps_pub)
def pose_cb(self, msg):
self.pose = msg
first_wpt_index = -1
min_wpt_distance = float('inf')
if self.waypoints is None:
return
num_waypoints_in_list = len(self.waypoints.waypoints)
# Generate an empty lane to store the final_waypoints
lane = Lane()
lane.header.frame_id = self.waypoints.header.frame_id
lane.header.stamp = rospy.Time(0)
lane.waypoints = []
# Iterate through the complete set of waypoints until we found the closest
distance_decreased = False
# rospy.loginfo('Started at waypoint index: %s', self.prev_first_wpt_index)
# start_time = time.time()
for index, waypoint in enumerate(
self.waypoints.waypoints[self.prev_first_wpt_index:] +
self.waypoints.waypoints[:self.prev_first_wpt_index],
start=self.prev_first_wpt_index):
current_wpt_distance = self.distance(self.pose.pose.position,
waypoint.pose.pose.position)
if distance_decreased and current_wpt_distance > min_wpt_distance:
break
if current_wpt_distance > 0 and current_wpt_distance < min_wpt_distance:
min_wpt_distance = current_wpt_distance
first_wpt_index = index
distance_decreased = True
first_wpt_index %= num_waypoints_in_list
transformed_light_point = None
# rospy.loginfo_throttle(1, 'Current waypoint: ' + str(self.prev_first_wpt_index) + ' of ' + str(len(self.waypoints.waypoints)) + ' ' + str(self.pose.pose.position.x))
if first_wpt_index == -1:
rospy.logwarn(
'WaypointUpdater::waypoints_cb - No waypoints ahead of ego were found... seems that the car went off course'
)
else:
# transform fast avoiding wait cycles
# Transform first waypoint to car coordinates
self.waypoints.waypoints[
first_wpt_index].pose.header.frame_id = self.waypoints.header.frame_id
try:
self.tf_listener.waitForTransform("base_link", "world",
rospy.Time(0),
rospy.Duration(0.02))
transformed_waypoint = self.tf_listener.transformPose(
"base_link",
self.waypoints.waypoints[first_wpt_index].pose)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException):
try:
self.tf_listener.waitForTransform(
"base_link", "world", rospy.Time(0),
rospy.Duration(TIMEOUT_VALUE))
transformed_waypoint = self.tf_listener.transformPose(
"base_link",
self.waypoints.waypoints[first_wpt_index].pose)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException):
rospy.logwarn("Failed to find camera to map transform")
return
# All waypoints in front of the car should have positive X coordinate in car coordinate frame
# If the closest waypoint is behind the car, skip this waypoint
if transformed_waypoint.pose.position.x <= 0.0:
first_wpt_index += 1
self.prev_first_wpt_index = first_wpt_index % num_waypoints_in_list
# Prepare for calculating speed:
slow_down = False
stop = False
reached_zero_velocity = False
car_distance_to_stop_line = -1.
# self.print_wp(self.waypoints.waypoints, first_wpt_index)
#self.print_wp(self.waypoints.waypoints, first_wpt_index)
# If red light, stop at the red light waypoint
if self.redlight_wp != -1:
# Find the distance to red light waypoint
car_distance_to_stop_line = self.distance_tl(
self.waypoints.waypoints, first_wpt_index,
self.redlight_wp)
# Compare car distance to min distance to make sure enough time to stop
if (car_distance_to_stop_line >
(STOP_DIST + STOP_TOL)) and (car_distance_to_stop_line <=
SLOW_DIST):
slow_down = True
rospy.loginfo('Slowing for red light')
# Use distance and current velocity to solve for average acceleration
decel = ((self.current_velocity /
(car_distance_to_stop_line - STOP_DIST)) * 0.6)
# TODO Add mode to wait at red light
# if within stopping distance, set future waypoints velocity to zero
elif (car_distance_to_stop_line <=
(STOP_DIST + STOP_TOL)) and (car_distance_to_stop_line >=
(STOP_DIST - STOP_TOL)):
stop = True
rospy.loginfo("Stopping at red light")
# Fill the lane with the final waypoints
for num_wp in range(LOOKAHEAD_WPS):
wp = Waypoint()
wp.pose = self.waypoints.waypoints[(first_wpt_index + num_wp) %
num_waypoints_in_list].pose
wp.twist = self.waypoints.waypoints[
(first_wpt_index + num_wp) % num_waypoints_in_list].twist
wp_max_velocity = wp.twist.twist.linear.x
# rospy.loginfo_throttle(10,'Waypoint max speed: ' + str(wp_max_velocity) + ' - ' + str(self.max_velocity) )
# Find velocity target based on stopping or not
if slow_down:
# Set all waypoints to same target velocity TODO may need calc each wp velocity
wp.twist.twist.linear.x = max(
0.0, self.current_velocity - decel)
elif stop:
# set velocity to zero
wp.twist.twist.linear.x = 0.0
else:
# wp.twist.twist.linear.x = wp_max_velocity
wp.twist.twist.linear.x = self.max_velocity
wp.twist.twist.linear.y = 0.0
wp.twist.twist.linear.z = 0.0
wp.twist.twist.angular.x = 0.0
wp.twist.twist.angular.y = 0.0
wp.twist.twist.angular.z = 0.0
lane.waypoints.append(wp)
# finally, publish waypoints as modified on /final_waypoints topic
self.final_waypoints_pub.publish(lane)
def decelerate_waypoints(self, waypoints, closest_idx):
temp = []
stop_idx = max(
self.stopline_wp_idx - closest_idx - self.stop_waypoint_margin, 0
) # Two waypoints back from stop line so front of car stops before stop line
last_braking_velocity = self.current_vel
new_dec_rate = TARGET_DECEL_RATE
#rospy.logwarn("")
#rospy.logwarn("*** Calculating decel waypoints: Stopline idx: {0}, Closest idx:{1}, Stop idx: {2}".format(self.stopline_wp_idx, closest_idx, stop_idx))
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
braking_vel = 0.0
dist = self.distance(waypoints, i, stop_idx)
# ** Maybe change this dist value to dist > 1.0 to prevent rolling forward?
if dist > 3.0 and last_braking_velocity < 1.0:
# Allow vehicle to accelerate from a dead stop if it is away from the stop line
# This is needed to get the car to start moving if the traffic light is red
# Calculate time to decelerate for the current distance at the deceleration rate
tts = math.sqrt(dist * 2.0 / new_dec_rate)
# Calculate braking velocity (a* t)
braking_vel = new_dec_rate * tts
# Clamp to maximum velocity parameter
braking_vel = min(braking_vel, wp.twist.twist.linear.x)
elif dist > 0.0 and not last_braking_velocity == 0.0:
tts = 0.0
if i == 0:
# Determine if the vehicle needs to decelerate at a rate faster than
# the TARGET_DECEL_RATE, which is typically caused by:
# - Missed classifications that cause a light state change
# - Late detection of a stop light
# - YELLOW to RED light transitions
# Calculate time to decelerate at the last known velocity
tts = dist * 2.0 / last_braking_velocity
# Calculate the deceleration rate needed to stop the vehicle from the
# last known velocity and time to stop
req_dec_rate = last_braking_velocity / tts
# Grab the higher decel rate - This will prevent pre-mature deceleration
req_dec_rate = max(req_dec_rate, TARGET_DECEL_RATE)
# Clamp the decel rate to the MAX_ACCEL parameter
new_dec_rate = min(req_dec_rate, self.max_decel)
else:
# Continue to decelerate at the last calculated decel rate
# Calculate time to decelerate for the current distance at the
# deceleration rate
tts = math.sqrt(dist * 2.0 / new_dec_rate)
# Calculate braking velocity (a * t)
braking_vel = new_dec_rate * tts
# Clamp to maximum velocity parameter
braking_vel = min(braking_vel, wp.twist.twist.linear.x)
# Hold car at a stop if the velocity is below 1.0 m/s
if braking_vel < 0.5:
braking_vel = 0.0
# Set last braking velocity to the new velocity so that this IF block can
# be bypassed on the rest of the waypoints
last_braking_velocity = braking_vel
# rospy.logwarn("Curr Vel: {0}, Braking Vel: {1}, Dist: {2}, New Decel Rate: {3}, \
# TTS: {4}" \
# .format(last_braking_velocity, braking_vel, dist, new_dec_rate, \
# tts))
# Original function
#vel = math.sqrt(2 * MAX_DECEL * dist)
#if vel < 0.5:
# vel = 0.0
p.twist.twist.linear.x = braking_vel
temp.append(p)
return temp
def loop(self):
rate = rospy.Rate(REFRESH_RATE)
while not rospy.is_shutdown():
rate.sleep()
if self.current_waypoints is None or self.current_pose is None:
continue
next_waypoint_index = self.next_infront_waypoint()
# set the target speed
target_speed = self.max_speed
# set the number of waypoints received from /base_waypoints
number_waypoints = len(self.current_waypoints)
lane = Lane()
lane.header.frame_id = self.current_pose.header.frame_id
lane.header.stamp = rospy.Time(0)
if (self.traffic_stop_waypoint_index != -1
and self.is_in_braking_distance()):
# we have decided that the waypoint published by the /traffic_waypoint
# is where we need to stop the car at
stop_waypoint_index = self.traffic_stop_waypoint_index
# TODO: handle wrapping
# Do we always generate LOOKAHEAD_WPS number of points? what to do
# if we will have points after the red traffic light? Just continue but
# set the velocity to 0 for those points?
decelerate_points_count = stop_waypoint_index - self.next_waypoint_index
if decelerate_points_count > 0:
for i in range(decelerate_points_count):
wp_new = Waypoint()
wp_extract = self.current_waypoints[
next_waypoint_index]
wp_new.pose = wp_extract.pose
lane.waypoints.append(wp_new)
wp_new.twist.twist.linear.x = target_speed
next_waypoint_index = (next_waypoint_index +
1) % number_waypoints
lane.waypoints = self.decelerate(lane.waypoints)
#generate up to the LOOKAHEAD_WPS number of waypoints
#fill it up with waypoints with zero velocity
if decelerate_points_count < LOOKAHEAD_WPS:
for i in range(LOOKAHEAD_WPS - decelerate_points_count):
wp_new = Waypoint()
wp_extract = self.current_waypoints[
next_waypoint_index]
wp_new.pose = wp_extract.pose
lane.waypoints.append(wp_new)
wp_new.twist.twist.linear.x = 0
else:
# now create the waypoints ahead
for i in range(LOOKAHEAD_WPS):
# create a new waypoint, rather than ammending existing waypoints
wp_new = Waypoint()
# extract the desired waypoint, starting at the next_waypoint_index
wp_extract = self.current_waypoints[next_waypoint_index]
# copy the position contents of the base_waypoint to the new waypoint
wp_new.pose = wp_extract.pose
# set the target velocity of the new waypoint
wp_new.twist.twist.linear.x = target_speed
# add to the Lane waypoints list
lane.waypoints.append(wp_new)
# then increment the next_waypoint_index, considering circlic nature of list
next_waypoint_index = (next_waypoint_index +
1) % number_waypoints
lane.waypoints = self.accelerate(lane.waypoints)
self.final_waypoints_pub.publish(lane)
def find_closest_waypoint_ahead_and_publish(self, waypoints):
first_wpt_index = -1
min_wpt_dist = float('inf')
dist_decreased = False
local_coord_x = None
num_waypoints = len(waypoints.waypoints)
for index, waypoint in enumerate(
waypoints.waypoints[self.former_first_wpt_index - 1:] +
waypoints.waypoints[:self.former_first_wpt_index - 1],
start=self.former_first_wpt_index - 1):
try:
tmp_wpt = waypoint
tmp_wpt.pose.header.frame_id = waypoints.header.frame_id
self.tf_listener.waitForTransform(
'/base_link', '/world', rospy.Time(0),
rospy.Duration(TIMEOUT_VALUE))
transformed_waypoint = self.tf_listener.transformPose(
'/base_link', tmp_wpt.pose)
current_wpt_dist = transformed_waypoint.pose.position.x**2 + transformed_waypoint.pose.position.y**2
if dist_decreased and current_wpt_dist > min_wpt_dist:
# we have already gone past the closest waypoint
rospy.loginfo(
'WaypointUpdater::waypoints_cb, found waypoint index %s',
first_wpt_index)
break
if current_wpt_dist < min_wpt_dist:
min_wpt_dist = current_wpt_dist
first_wpt_index = index
local_x_coord = transformed_waypoint.pose.position.x
dist_decreased = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr(
'WaypointUpdater::waypoints_cb - could not get coordinate frame transform!!!'
)
first_wpt_index %= num_waypoints
if first_wpt_index == -1:
rospy.logwarn(
'WaypointUpdater::waypoints_cb - No waypoints ahead of ego were found... seems that the car went off course'
)
else:
if local_coord_x <= 0.0:
first_wpt_index += 1
self.former_first_wpt_index = first_wpt_index % num_waypoints
lane = Lane()
lane.header.frame_id = waypoints.header.frame_id
lane.header.stamp = rospy.Time(0)
lane.waypoints = []
for num_wp in range(LOOKAHEAD_WPS):
wp = Waypoint()
wp.pose = waypoints.waypoints[(first_wpt_index + num_wp) %
num_waypoints].pose
wp.twist = waypoints.waypoints[(first_wpt_index + num_wp) %
num_waypoints].twist
wp.twist.twist.linear.x = self.default_velocity
wp.twist.twist.linear.y = 0.0
wp.twist.twist.linear.z = 0.0
wp.twist.twist.angular.x = 0.0
wp.twist.twist.angular.y = 0.0
wp.twist.twist.angular.z = 0.0
lane.waypoints.append(wp)
self.final_waypoints_pub.publish(lane)
rospy.loginfo(
'waypoint_updater - publishing /final_waypoints with %s waypoints and first waypoint with index %s',
len(lane.waypoints), first_wpt_index)
def publish_waypoints(self, idx):
idx = max(0, idx)
end_idx = min(idx + LOOKAHEAD_WPS,
len(self.base_waypoints.waypoints) - 1)
stopline_wp_idx = self.stopline_wp_idx
stopping = False
if self.published_waypoints is not None and self.last_stopline_wp_idx == stopline_wp_idx\
and abs(idx - self.published_waypoints_offset) < 5:
#reuse and extend the waypoints
used_up = idx - self.published_waypoints_offset
self.published_waypoints = self.published_waypoints[used_up:]
self.published_waypoints_offset = idx
if len(self.published_waypoints) < LOOKAHEAD_WPS:
#cruising and we should copy over some waypoints
first_copied = idx + len(self.published_waypoints)
if first_copied < len(self.base_waypoints.waypoints):
self.published_waypoints.extend(
self.base_waypoints.waypoints[first_copied:end_idx])
else:
#either stopping, standing still or starting or manual control
current_velocity = self.current_velocity
self.last_stopline_wp_idx = stopline_wp_idx
if stopline_wp_idx > idx:
end_idx = stopline_wp_idx
dist_stop = max(
self.distance(self.base_waypoints.waypoints, idx, end_idx),
2)
x = [dist_stop]
y = [current_velocity]
stopping = True
x.append(0.75 * dist_stop)
y.append(0.75 * current_velocity)
x.append(0.1 * dist_stop)
y.append(0.15 * current_velocity)
if 0.1 * dist_stop > 4.0 and 0.75 * dist_stop > 4.0:
x.append(4.0)
y.append(0.0)
x.append(0.0)
y.append(0.0)
x.append(-1)
y.append(0.0)
else:
start_dist = max(
self.distance(self.base_waypoints.waypoints, idx, end_idx),
2)
x = [start_dist + 1, start_dist]
current_velocity = max(current_velocity, 2)
y = [current_velocity, current_velocity]
target_velocity = min(
self.MAX_VELOCITY, self.base_waypoints.waypoints[end_idx].
twist.twist.linear.x)
diff_vel = target_velocity - current_velocity
safe_accel = 0.6 * self.MAX_ACCEL
t = abs(diff_vel / safe_accel)
dist_t = start_dist - (
t * current_velocity +
math.copysign(0.5 * safe_accel, diff_vel) * t**2)
x.append(dist_t)
y.append(target_velocity)
if dist_t < 0:
x.append(dist_t - 10)
y.append(target_velocity)
else:
x.append(0)
y.append(target_velocity)
x.append(-1.0)
y.append(y[-1])
x.reverse()
y.reverse()
fixed_speed = False
rospy.logwarn(
"waypoint_updater: stopping:{0} fixed_speed:{1} stopline_idx:{2} x:{3} y:{4}"
.format(stopping, fixed_speed, stopline_wp_idx, x, y))
if (not stopping and abs(y[-1] - y[0]) < 0.5):
fixed_speed = y[0]
else:
spline_rep = interpolate.splrep(x, y, s=0.1)
new_wps = []
log_vel = []
for i in range(idx, end_idx):
p = Waypoint()
p.pose = self.base_waypoints.waypoints[i].pose
dist = self.distance(self.base_waypoints.waypoints, i, end_idx)
if fixed_speed:
vel = fixed_speed
else:
vel = interpolate.splev(dist, spline_rep, der=0).sum()
if stopping and abs(dist) < 4:
vel = 0.0
elif vel < 0.1:
vel = 0.0
p.twist.twist.linear.x = vel
log_vel.append(vel)
new_wps.append(p)
self.published_waypoints = new_wps
self.published_waypoints_offset = idx
rospy.logwarn("waypoint_updater: velocities:{0} ".format(log_vel))
lane = Lane()
lane.waypoints = self.published_waypoints
self.final_waypoints_pub.publish(lane)
def generate_waypoint(self, waypoint, vel_cmd):
p = Waypoint()
p.pose = waypoint.pose
p.twist.twist.linear.x = vel_cmd
return p
def pose_cb(self, msg):
self.pose = msg
first_wpt_index = -1
min_wpt_distance = float('inf')
if self.waypoints is None:
return
num_waypoints_in_list = len(self.waypoints.waypoints)
# Gererate an empty lane to store the final_waypoints
lane = Lane()
lane.header.frame_id = self.waypoints.header.frame_id
lane.header.stamp = rospy.Time(0)
lane.waypoints = []
# Iterate through the complete set of waypoints until we found the closest
distance_decreased = False
#rospy.loginfo('Started at waypoint index: %s', self.prev_first_wpt_index)
#start_time = time.time()
for index, waypoint in enumerate(
self.waypoints.waypoints[self.prev_first_wpt_index:] +
self.waypoints.waypoints[:self.prev_first_wpt_index],
start=self.prev_first_wpt_index):
current_wpt_distance = self.distance(self.pose.pose.position,
waypoint.pose.pose.position)
if distance_decreased and current_wpt_distance > min_wpt_distance:
break
if current_wpt_distance > 0 and current_wpt_distance < min_wpt_distance:
min_wpt_distance = current_wpt_distance
first_wpt_index = index
distance_decreased = True
first_wpt_index %= num_waypoints_in_list
transformed_light_point = None
if first_wpt_index == -1:
rospy.logwarn(
'WaypointUpdater::waypoints_cb - No waypoints ahead of ego were found... seems that the car went off course'
)
else:
#transform fast avoiding wait cycles
# Transform first waypoint to car coordinates
self.waypoints.waypoints[
first_wpt_index].pose.header.frame_id = self.waypoints.header.frame_id
try:
self.tf_listener.waitForTransform("base_link", "world",
rospy.Time(0),
rospy.Duration(0.02))
transformed_waypoint = self.tf_listener.transformPose(
"base_link",
self.waypoints.waypoints[first_wpt_index].pose)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException):
try:
self.tf_listener.waitForTransform(
"base_link", "world", rospy.Time(0),
rospy.Duration(TIMEOUT_VALUE))
transformed_waypoint = self.tf_listener.transformPose(
"base_link",
self.waypoints.waypoints[first_wpt_index].pose)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException):
rospy.logwarn("Failed to find camera to map transform")
return
# All waypoints in front of the car should have positive X coordinate in car coordinate frame
# If the closest waypoint is behind the car, skip this waypoint
if transformed_waypoint.pose.position.x <= 0.0:
first_wpt_index += 1
self.prev_first_wpt_index = first_wpt_index % num_waypoints_in_list
# Prepare for calculating velocity:
slow_down = False
reached_zero_velocity = False
car_distance_to_stop_line = -1.
planned_velocity = self.default_velocity
# If the last traffic_waypoint message is newer than the threshold, we might need to the car.
if self.light_waypoint_index >= 0:
rospy.logdebug('should stopp the car %s',
self.light_waypoint_index)
self.waypoints.waypoints[
self.
light_waypoint_index].pose.header.frame_id = self.waypoints.header.frame_id
transformed_light_point = self.tf_listener.transformPose(
"base_link",
self.waypoints.waypoints[self.light_waypoint_index].pose)
# The approximate distance from the stop line to the traffic light
car_distance_to_stop_line = transformed_light_point.pose.position.x - self.light_distance_thresh
# Estimate whether the car cannot cross the stop line on yellow (in less than 2 seconds). Otherwise don't slow down.
if self.velocity / car_distance_to_stop_line < 2 and car_distance_to_stop_line >= 4:
slow_down = True
if self.car_distance_to_sl_when_car_started_to_slow_down is None:
self.car_distance_to_sl_when_car_started_to_slow_down = car_distance_to_stop_line
self.car_velocity_when_car_started_to_slow_down = self.velocity
rospy.logdebug('Stopping the car')
planned_velocity = min(
max(abs(car_distance_to_stop_line * 0.2), 0.0),
self.default_velocity)
# Stop the car in a safe distance before the stop line to give the simulator space to adapt velocity
#we are close to the stop line and slow
elif car_distance_to_stop_line > 0 and car_distance_to_stop_line < 4 and self.velocity < 6:
slow_down = True
if car_distance_to_stop_line > 0.5:
planned_velocity = 1.0
else:
planned_velocity = 0.0
reached_zero_velocity = True
else:
rospy.logwarn('too late to stopp the car')
self.car_distance_to_tl_when_car_started_to_slow_down = None
self.car_velocity_when_car_started_to_slow_down = None
rospy.loginfo(
'car_distance_to_stop_line %s velocity %s set to %s',
car_distance_to_stop_line, self.velocity, planned_velocity)
# Fill the lane with the final waypoints
for num_wp in range(LOOKAHEAD_WPS):
wp = Waypoint()
wp.pose = self.waypoints.waypoints[(first_wpt_index + num_wp) %
num_waypoints_in_list].pose
wp.twist = self.waypoints.waypoints[
(first_wpt_index + num_wp) % num_waypoints_in_list].twist
wp.twist.twist.linear.x = planned_velocity
wp.twist.twist.linear.y = 0.0
wp.twist.twist.linear.z = 0.0
wp.twist.twist.angular.x = 0.0
wp.twist.twist.angular.y = 0.0
wp.twist.twist.angular.z = 0.0
lane.waypoints.append(wp)
# finally, publish waypoints as modified on /final_waypoints topic
self.final_waypoints_pub.publish(lane)
def publish_waypoints(self, start):
'''
Publishes a message of type :py:class:Lane containing the points
ahead the last know pose :py:attr:pose.
'''
lane = Lane()
#closest_index = self.get_closest_waypoints()
closest_index = start
farthest_index = closest_index + LOOKAHEAD_WPS
rospy.loginfo(
"closest_index = %d farthest_index = %d self.stopline_waypoint_index = %d",
closest_index, farthest_index, self.stopline_waypoint_index)
# rospy.loginfo("closest_index = %d", self.linear_search())
lane.header = self.base_waypoints.header
if self.stopline_waypoint_index == -1 or (self.stopline_waypoint_index
>= farthest_index):
lane.waypoints = self.base_waypoints.waypoints[
closest_index:farthest_index]
# rospy.loginfo("farthest_index=%d length=%d", farthest_index, len(self.base_waypoints.waypoints))
# for i in range(len(lane.waypoints)):
# velocity = lane.waypoints[i].twist.twist.linear.x
# # rospy.loginfo("Velocity=%2.2f", velocity)
else:
# decelerate
lane.waypoints = []
# stop a little before the stop line to account for falf of the length of the car
stop_index = max(self.stopline_waypoint_index - 2, 0)
# try to stop smoothly in 30 waypoints from the stop line
v_index = max(0, stop_index - 30)
v0 = self.get_waypoint_velocity(
self.base_waypoints.waypoints[v_index])
x0 = self.distance(self.base_waypoints.waypoints, v_index,
stop_index) / 2.0
rospy.loginfo("x0=%f v0=%f", x0, v0)
for i in range(closest_index, stop_index):
waypoint = Waypoint()
waypoint.pose = self.base_waypoints.waypoints[i].pose
# calculate velocity as a sgmoid function
dist = self.distance(self.base_waypoints.waypoints, i,
stop_index)
exp_term = math.exp(-0.2 * (dist - x0))
velocity = v0 / (1 + exp_term)
if (i == stop_index - 1):
velocity = 0.0
# do not exceed logitudinal acceleration limits
max_vel = math.sqrt(2 * self.decel_limit_mpsps * dist)
if max_vel < 1.:
max_vel = 0.0
rospy.loginfo("dist=%2.2f velocity=%2.2f max_vel=%2.2f", dist,
velocity, max_vel)
velocity = min(max_vel, velocity)
waypoint.twist.twist.linear.x = min(
velocity,
self.base_waypoints.waypoints[i].twist.twist.linear.x)
lane.waypoints.append(waypoint)
#set the velocity of the last point to 0.0
#lane.waypoints[ len(lane.waypoints) - 1 ] = 0.0
self.final_waypoints_pub.publish(lane)
def _get_waypoint(self, value=0.0):
waypoint = Waypoint()
waypoint.pose = self._get_posestamped(value)
waypoint.twist = self._get_twiststamped(0.0)
return waypoint
def copy_waypoint_pose(wp):
new_wp = Waypoint()
new_wp.pose = wp.pose
new_wp.twist.twist.linear.x = wp.twist.twist.linear.x
return new_wp
def decelerate_waypoints(self, waypoints, closest_idx):
print("Waypoints: ")
# Make a new list of waypoints in order to store the manipulated base waypoints #
temp = []
# Two waypoints back from line so front of car stops approximately at the line #
stop_idx = max(self.stopline_wp_idx - closest_idx - 2, 0)
# Unused attempts at linear decelleration #
# Calculate the distance between the first waypoint and the index at which the vehicle has to come to a standstill #
dist_tot = self.distance(waypoints, 0, stop_idx)
v_start = self.current_vel # self.pose.twist.twist.linear.x # self.distance(waypoints, 0, stop_idx)
# Calculate the time required to come to a standstill at the stop index assuming linear unlimited acceleration #
#t_tot = 2.0 * dist_tot / v_start
# Calculate the necessary acceleration ("deceleration") across the entire section #
#accel = -1.0 * min(MAX_DECEL, ( v_start / t_tot ) )
# Calculate the distance to come to a standstill #
#dist_stop = v_start*v_start / ( 2.0 * (-1.0 * accel) )
# Calculate the corresponding acceleration per meter #
#accel_m = v_start / dist_stop
#a_would_be = v_start * v_start / ( 2.0 * dist_tot )
#if MAX_DECEL <= a_would_be:
#for i in range(stop_idx, len(waypoints)):
#d_would_be = self.distance(waypoints, 0, i)
#a_would_be = v_start * v_start / ( 2.0 * d_would_be )
#if a_would_be <= MAX_DECEL:
#stop_idx = i
#break
#d_stop = v_start * v_start / ( 2.0 * a_would_be )
#a_d = v_start / d_stop
# -------------------------------------- #
# Calculate the corresponding acceleration per waypoint #
for i, wp in enumerate(waypoints):
p = Waypoint()
# The goal position and orientation of the vehicle at the waypoint remains the same #
p.pose = wp.pose
# Calculate the distance between this waypoint and the index at which the vehicle has to come to a standstill #
dist = self.distance(waypoints, i, stop_idx)
# Calculate the velocity the vehicle has to have at this waypoint to come to a standstill in time #
#vel = math.sqrt(2.0 * MAX_DECEL * dist)
#vel = v_start - ( (dist_tot - dist) * accel_m )
#vel = v_start - ( a_d * (d_stop-dist) )
delta_dist = dist_tot - dist
if 0.0 < delta_dist:
vel = v_start * (1 - (delta_dist / dist_tot) *
(delta_dist / dist_tot))
else:
vel = v_start
if vel < 0.1:
vel = 0.0
# A safeguard to arrive at the destination despite of an over-enthusiastic controller #
if i < (stop_idx - 2) and v_start < 0.2:
vel = 0.75
#if i == stop_idx - 5:
#vel = 0.3
#else:
#if i == stop_idx - 4:
#vel = 1.0
#else:
#vel = 1.5
else:
if (stop_idx - 2) < i:
vel = 0.0
# Use the calculated required waypoint velocity in stead of the original/base waypoint velocity, unless the base waypoint velocity is smaller than this value #
p.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
temp.append(p)
return temp
def publish_waypoints(self):
closest_wp_idx = self.closest_wp_idx
start_point_velocity = self.get_waypoint_velocity(
self.waypoints[closest_wp_idx])
# get the distance from each waypoint till the stop line
distance_to_stop_line = self.distances_to_end(
self.waypoints[closest_wp_idx:self.stop_idx])
farthest_wp_idx = self.closest_wp_idx + LOOKAHEAD_WPS
lane = Lane()
waypoints_ahead = []
print(self.driving_state)
if self.driving_state == DRIVE_STATE_STOPPING and closest_wp_idx < self.stop_idx:
# Loop over the waypoints up to next_red_light waypoint
# setting desired velocity at each waypoint as a ratio of remaining distance to stop
# from the respective waypoint to the total distance from current position
# to stop line
for i in range(0, (abs(closest_wp_idx - self.stop_idx))):
# get the distance to the i-th way point
# i_point_distance = self.distances_to_end(self.waypoints, self.closest_waypoint, i)
p = Waypoint()
p.pose = self.waypoints[closest_wp_idx + i].pose
p.twist = self.waypoints[closest_wp_idx + i].twist
if (distance_to_stop_line[0]) > STOP_LINE_OFFSET:
if self.current_velocity < 0.8:
i_point_target_velocity = -10.0
else:
i_point_target_velocity = distance_to_stop_line[
i] / distance_to_stop_line[0]
i_point_target_velocity = (start_point_velocity *
i_point_target_velocity)
else:
i_point_target_velocity = -10.0 # negative stops car 'creep' when stopped
p.twist.twist.linear.x = i_point_target_velocity
waypoints_ahead.append(self.waypoints[closest_wp_idx + i])
for i in range(self.stop_idx, farthest_wp_idx):
waypoints_ahead.append(self.waypoints[i])
lane.waypoints = waypoints_ahead
elif self.driving_state == DRIVE_STATE_DRIVING:
# Set the velocity to reference velocity if driving state is not stopping
# print("in else")
lane.waypoints = self.waypoints[closest_wp_idx:farthest_wp_idx]
# print(lane.waypoints)
else:
pass
# now publish the waypoints
# get LOOKAHEAD_WPS number of waypoints ahead of the car
# n_waypoints = len(self.waypoints) # can only get this many waypoints
# if n_waypoints > LOOKAHEAD_WPS:
# n_waypoints = LOOKAHEAD_WPS # max waypoints to pass over
# for i in range(n_waypoints):
# # check that the waypoints we want are in the range of the waypoint array
# if closest_wp_idx + i < len(self.waypoints):
# waypoints_ahead.append(self.waypoints[closest_wp_idx + i])
# structure the data to match the expected styx_msgs/Lane form
# lane.waypoints = waypoints_ahead # list of waypoints ahead of the car
# lane.header.stamp = rospy.Time(0) # timestamp
self.final_waypoints_pub.publish(lane)
