def decelerate_waypoints(self, waypoints, closest_idx):
result = []
# How many waypoints ahead we want to stop.
stop_idx = max(self.stopline_wp_idx - closest_idx - 3, 0)
SCALING_FACTOR = 10 # change the deceleration profile - higher values will make the car decelerate only when closer to the traffic light
dist = self.distance(waypoints, 0, stop_idx)
max_allowed_vel = math.sqrt(SCALING_FACTOR * MAX_DECEL * dist)
# rospy.logwarn(
# "max_vel_to_stop: {}, curr_vel: {}".format(
# max_allowed_vel, self.current_vel
# )
# )
if max_allowed_vel > self.current_vel:
return waypoints
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = wp.pose
p.twist = copy.deepcopy(wp.twist)
dist = self.distance(waypoints, i, stop_idx)
vel = math.sqrt(SCALING_FACTOR * MAX_DECEL * dist)
vel = max(0.0, vel)
p.twist.twist.linear.x = min(vel, p.twist.twist.linear.x)
result.append(p)
return result
def publish(self):
if self.waypoints is None or self.pose 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
#rospy.loginfo('Started at waypoint index: %s', self.prev_first_wpt_index)
#start_time = time.time()
first_wpt_index = -1
min_wpt_distance = float('inf')
for index, waypoint in enumerate(self.waypoints.waypoints):
current_wpt_distance = self.distance(self.pose.pose.position, waypoint.pose.pose.position)
if current_wpt_distance > 0 and current_wpt_distance < min_wpt_distance:
min_wpt_distance = current_wpt_distance
first_wpt_index = index
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.05))
transformed_waypoint = self.tf_listener.transformPose("base_link", self.waypoints.waypoints[first_wpt_index].pose)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logwarn("fail %s", time.time())
return
if transformed_waypoint.pose.position.x <= 0.0:
first_wpt_index += 1
first_wpt_index %= num_waypoints_in_list
planned_velocity = 4.4
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
if self.light_waypoint_index > -1:
planned_velocity -= 0.1
if planned_velocity < 0.1:
planned_velocity = 0
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(self):
# Define container for waypoints ahead
wps_ahead = Lane()
# Determine length of input waypoints
waypoints_len = len(self.base_waypoints)
# Determine first waypoint ahead of the car
# Searching for closest waypoint (Just ID)
idx_wp_closest = self.get_idx_closest_waypoint()
# Seeing if closest waypoint is ahead, otherwise choose the next (Just ID)
idx_wp_ahead = self.get_idx_ahead_waypoint(idx_wp_closest)
# Setting last_waypoint_id now
self.last_waypoint_id = idx_wp_ahead
# Determine waypoints ahead to be published
idx_cur = idx_wp_ahead
# Going through the whole list of traffic points we want to publish
for i in range(LOOKAHEAD_WPS):
# Setting the pose and twist data for the waypoints in the list
wp = self.base_waypoints[idx_cur]
next_wp = Waypoint()
next_wp.pose = wp.pose
next_wp.twist = wp.twist
# Resetting our speed for the waypoints, if the car is getting from stopping to driving again
if (self.traffic_light_helper > 0) & (self.drive > 0):
next_wp.twist.twist.linear.x = self.get_waypoint_velocity(
self.base_waypoints_copy[idx_cur])
# Append the waypoint to wps_ahead
wps_ahead.waypoints.append(next_wp)
# For the wrap-around last waypoint to first waypoint
idx_cur = (idx_cur + 1) % waypoints_len
# Setting drive-parameter when we want to driving again
if (self.traffic_light_helper > 0) & (self.drive > 0):
self.drive = 0
# Stopping if a traffic_light is red in front of us
if self.tf_state > -1:
self.stopping(self.tf_state, idx_wp_ahead, wps_ahead)
# Setting wps_ahead to self.wps_ahead
self.wps_ahead = wps_ahead
# Publish our waypoints!
self.final_waypoints_pub.publish(wps_ahead)
def publish(self):
wps_pub = Lane()
self.find_next_waypoint()
#determine the list of way points to publish
if self.next_waypoint_index is not None:
wp_index = self.next_waypoint_index
for i in range(LOOKAHEAD_WPS):
start_wp = self.base_waypoints[wp_index]
next_wp = Waypoint()
next_wp.pose = start_wp.pose
next_wp.twist = start_wp.twist
wps_pub.waypoints.append(next_wp)
wp_index = (wp_index + 1) % self.num_waypoints
#rospy.logwarn('way points published:{}'.format(len(wps_pub.waypoints)))
self.final_waypoints_pub.publish(wps_pub)
def get_final_waypoints(self, waypoints, start_wp, end_wp):
final_waypoints = []
if end_wp < start_wp: # deal with the corner case that next traffic way point crossed the end of the track
end_wp += len(waypoints)
for i in range(start_wp, end_wp + 1):
wp = Waypoint()
index = i % len(waypoints)
wp.pose = copy.deepcopy(waypoints[index].pose)
wp.twist = copy.deepcopy(waypoints[index].twist)
final_waypoints.append(wp)
if self.braking:
final_waypoints = self.decelerate(final_waypoints)
else:
final_waypoints = self.accelerate(final_waypoints)
return final_waypoints
def publish(self):
wps_pub = Lane()
self.find_next_waypoint()
#determine the list of way points to publish
distance_tl = None
#check red light distance
#TODO will it detect yellow light?
if self.red_light_index and self.next_waypoint_index:
#distance to the next traffic light
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 red 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 2 seconds
if distance_tl and distance_tl / self.current_velocity > 2:
set_speed = 0
else:
set_speed = self.max_velocity_km
if self.next_waypoint_index is not None:
wp_index = self.next_waypoint_index
for i in range(LOOKAHEAD_WPS):
start_wp = self.base_waypoints[wp_index]
next_wp = Waypoint()
next_wp.pose = start_wp.pose
next_wp.twist = start_wp.twist
next_wp.twist.twist.linear.x = set_speed
wps_pub.waypoints.append(next_wp)
wp_index = (wp_index+1) % self.num_waypoints
#rospy.logwarn('way points published:{}'.format(len(wps_pub.waypoints)))
self.final_waypoints_pub.publish(wps_pub)
def publish(self):
# Define container for waypoints ahead
wps_ahead = Lane()
# Determine length of input waypoints
waypoints_len = len(self.base_waypoints)
# Determine first waypoint ahead of the car
idx_wp_closest = self.get_idx_closest_waypoint()
idx_wp_ahead = self.get_idx_ahead_waypoint(idx_wp_closest)
self.last_waypoint_id = idx_wp_ahead
# Determine waypoints ahead to be published
idx_cur = idx_wp_ahead
#rospy.loginfo(idx_cur)
for i in range(LOOKAHEAD_WPS):
wp = self.base_waypoints[idx_cur]
next_wp = Waypoint()
next_wp.pose = wp.pose
next_wp.twist = wp.twist
#if (self.need > 200) & (self.drive > 0):
if (self.need > 0) & (self.drive > 0):
next_wp.twist.twist.linear.x = self.save_velocity
wps_ahead.waypoints.append(next_wp)
idx_cur = (idx_cur + 1) % waypoints_len
#if (self.need > 200) & (self.drive > 0):
if (self.need > 0) & (self.drive > 0):
self.drive = 0
#self.stopping(400, idx_wp_ahead, wps_ahead)
#self.stopping(753, idx_wp_ahead, wps_ahead)
#rospy.loginfo(self.tf_state)
#rospy.loginfo(self.need)
if self.tf_state > 300:
#rospy.loginfo(self.tf_state)
self.stopping(self.tf_state, idx_wp_ahead, wps_ahead)
#rospy.loginfo(self.need)
# Publish waypoints ahead
self.wps_ahead = wps_ahead
self.final_waypoints_pub.publish(wps_ahead)
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(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 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)
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 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)