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 loop(self):
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if (self.saved_base_waypoints is not None) and (self.current_pose is not None):
#check waypoints availability
if self.saved_base_waypoints is None:
return
# seach for the closest point
closest_waypoint, closest_index = self.find_closest_waypoint_with_index()
#Setup test publish
lane = Lane()
for i in range(LOOKAHEAD_WPS):
idx = (closest_index+i) % self.num_waypoints
wp = self.saved_base_waypoints.waypoints[idx]
new_waypoint = Waypoint()
new_waypoint.pose.pose.position = wp.pose.pose.position
#set velocity for each point
new_waypoint.twist.twist.linear.x = self.speed
if self.stop_wp_inx is not None:
if (self.stop_wp_inx is not None) and (i <= self.stop_wp_inx):
sidx = self.stop_wp_inx
stopdist = self.distance(self.saved_base_waypoints.waypoints, idx, sidx)
currVel = self.current_velocity.twist.linear.x
if stopdist < 2.*currVel*currVel/(2.* MAX_DEACC):
new_waypoint.twist.twist.linear.x = 0.
else:
if stopdist < 4.*currVel*currVel/(2.* MAX_DEACC):
new_waypoint.twist.twist.linear.x = self.speed/2.
else:
new_waypoint.twist.twist.linear.x = self.speed
else:
new_waypoint.twist.twist.linear.x = 0.
lane.waypoints.append(new_waypoint)
self.final_waypoints_pub.publish(lane)
rate.sleep()
def slow_down(self, next, down_to, slow_start=False):
stopping_waypoints = []
if next < down_to:
for wp in self.base_waypoints[next:down_to + 1]:
p = Waypoint()
wp_pose = wp.pose.pose
p.pose.pose.position.x = wp_pose.position.x
p.pose.pose.position.y = wp_pose.position.y
p.pose.pose.position.z = wp_pose.position.z
q = self.quaternion_from_yaw(0.0)
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = SLOW_START_SPEED if slow_start else wp.twist.twist.linear.x
stopping_waypoints.append(p)
return self.decelerate(
stopping_waypoints) if len(stopping_waypoints) > 0 else []
def load_waypoints(self, fname):
waypoints = []
with open(fname) as wfile:
reader = csv.DictReader(wfile, CSV_HEADER)
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = self.quaternion_from_yaw(float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = float(self.velocity)
waypoints.append(p)
# (vhavrylov) : the car is able to commute more than 1 lap
# only if I disable deceleration at the end of the waypoint
# list
#return self.decelerate(waypoints)
return waypoints
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
rospy.Subscriber('/traffic_waypoint', Lane, self.traffic_cb)
rospy.Subscriber('/obstacle_waypoints', Lane, self.obstacle_cb)
self.final_waypoints_pub = rospy.Publisher('final_waypoints',
Lane,
queue_size=1)
# TODO: Add other member variables you need below
self.vehiclePose = PoseStamped()
self.baseWayPoints = Lane()
self.waypoint = Waypoint()
self.nxtWayPoint = 0
self.final_waypoints = Lane()
self.ID = 0
rospy.spin()
def get_final_waypoints(self, waypoints, start_wp, end_wp, mode='const'):
final_waypoints = []
for i in range(start_wp, end_wp):
index = i % len(waypoints)
wp = Waypoint()
wp_x, wp_y, wp_z = get_position(waypoints[index])
wp.pose.pose.position.x = wp_x
wp.pose.pose.position.y = wp_y
wp.pose.pose.position.z = wp_z
wp.pose.pose.orientation = waypoints[index].pose.pose.orientation
if mode == 'const':
wp.twist.twist.linear.x = waypoints[index].twist.twist.linear.x
elif mode == 'sine':
wp.twist.twist.linear.x = self.get_sine_speed(index)
elif mode == 'zero':
wp.twist.twist.linear.x = 0
final_waypoints.append(wp)
return final_waypoints
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
light = None
light_idx = None
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions = self.config['stop_line_positions']
if(self.pose):
# Find the car position from waypoint list
car_position_idx = self.get_closest_waypoint(self.pose.pose.position, self.waypoints.waypoints)
# Find the nearest traffic light to car from traffic light list
light_idx = self.get_closest_waypoint(self.waypoints.waypoints[car_position_idx].pose.pose.position, self.lights)
#TODO find the closest visible traffic light (if one exists)
light_waypoint_idx = self.get_closest_waypoint(self.lights[light_idx].pose.pose.position, self.waypoints.waypoints)
light_position = self.waypoints.waypoints[light_waypoint_idx].pose.pose.position
light = self.lights[light_idx]
stop_light_line = self.config['stop_line_positions'][light_idx]
stop_line = Waypoint()
stop_line.pose.pose.position.x = stop_light_line[0]
stop_line.pose.pose.position.y = stop_light_line[1]
stop_line.pose.pose.position.z = 0.
stop_line_index = self.get_closest_waypoint(stop_line.pose.pose.position, self.waypoints.waypoints)
stop_line_waypoint = self.waypoints.waypoints[stop_line_index].pose.pose.position
if light and (stop_line_index > car_position_idx):
state, confidence = self.get_light_state(light)
state_ground_truth = self.lights[light_idx].state
if state != self.light_state:
self.light_state = state
#rospy.logwarn("light changed from %d to %d (with confidence: %f) and ground truth is %d", self.light_state, state, confidence, state_ground_truth)
return stop_line_index, self.light_state
return -1, TrafficLight.UNKNOWN
def filterWaypoints(self, wp):
if wp.waypoints[0].pose.pose.position.x == 10.4062:
waypoints = []
path = rospy.get_param('~path')
if not os.path.isfile(path):
return wp.waypoints
with open(path) as wfile:
reader = csv.DictReader(wfile, ['x','y','z','yaw'])
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = tf.transformations.quaternion_from_euler(0., 0., float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = 2.7777778
waypoints.append(p)
return waypoints
return wp.waypoints
def load_waypoints(self, fname):
waypoints = []
with open(fname) as wfile:
reader = csv.DictReader(wfile, CSV_HEADER)
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = self.quaternion_from_yaw(float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = float(self.velocity)
waypoints.append(p)
if not self.loop:
return self.decelerate(waypoints)
else:
return waypoints
def test_get_cross_track_error_from_frenet():
TestWaypoints = Lane()
xrange = np.arange(0, 100, 0.01)
yrange = np.arange(0, 100, 0.01)
currentPose = PoseStamped()
currentPose.pose.position.x = 20
currentPose.pose.position.y = 10
for idx,xval in enumerate(xrange):
wp = Waypoint()
wp.pose.pose.position.x = xrange[idx]
wp.pose.pose.position.y = yrange[idx]
wp.twist.twist.linear.x = 20 # longitudinal velocity
wp.twist.twist.linear.y = 0 # lateral velocity
TestWaypoints.waypoints.append(wp)
cte = get_cross_track_error_from_frenet(TestWaypoints.waypoints, currentPose.pose)
assert cte == -10*np.cos(math.pi/4)
def get_next_waypoints(self):
'''returns the next waypoints.'''
next_waypoints = []
all_waypoints = self.base_waypoints
start_index = self.current_car_index
end_index = self.current_car_index + self.num_look_ahead_waypoints
for i in range(start_index, end_index):
wp = Waypoint()
index = i % len(all_waypoints)
wp.pose.pose.position = all_waypoints[index].pose.pose.position
wp.pose.pose.orientation = all_waypoints[
index].pose.pose.orientation
wp.twist.twist.linear.x = all_waypoints[index].twist.twist.linear.x
next_waypoints.append(wp)
return next_waypoints
def load_waypoints(self, fname, direction):
clockwise = True
if direction == 'anticlockwise':
clockwise = False
waypoints = []
with open(fname) as wfile:
reader = csv.DictReader(wfile, CSV_HEADER)
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = self.quaternion_from_yaw(float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = float(self.velocity)
waypoints.append(p)
if clockwise == False:
rospy.loginfo('Waypoints anticlockwise')
waypoints.reverse()
return self.decelerate(waypoints)
def load_waypoints(self, fname):
waypoints = []
with open(fname) as wfile:
reader = csv.DictReader(wfile, CSV_HEADER)
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = self.quaternion_from_yaw(float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
# km/h to m/s
# Clamping speed in launch file
# For extra safety, we provide redundant clamping
# 10mph <-> 16Km/h
if self.velocity >= 10:
self.velocity = 10
p.twist.twist.linear.x = float(self.mph_to_mps(self.velocity))
waypoints.append(p)
return self.decelerate(waypoints)
def load_waypoints(self, fname):
waypoints = []
with open(fname) as wfile:
reader = csv.DictReader(wfile, CSV_HEADER)
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = self.quaternion_from_yaw(float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
if float(self.velocity) < 15.0:
p.twist.twist.linear.x = float(self.velocity)
else:
rospy.logwarn(
"waypoint_loader: Velocity param is exceed than max value {:3f}"
.format(15.0))
p.twist.twist.linear.x = 15.0
waypoints.append(p)
return self.decelerate(waypoints)
def determine_final_waypoints(self, start_wp):
self.final_waypoints = []
if self.stop == 1:
for i in range(start_wp, self.traffic_waypoint):
j = i % len(self.base_waypoints.waypoints)
wp_new = self.get_final_waypoint(i, 0)
self.final_waypoints.append(wp_new)
target_wp = len(self.final_waypoints)
i_max = max(start_wp + LOOKAHEAD_WPS, self.traffic_waypoint + 1)
for i in range(self.traffic_waypoint, i_max):
wp_new = self.get_final_waypoint(i, 1)
self.final_waypoints.append(wp_new)
last = self.final_waypoints[target_wp]
last.twist.twist.linear.x = 0.0
for wp in self.final_waypoints[:target_wp][::-1]:
x_diff = wp.pose.pose.position.x - last.pose.pose.position.x
y_diff = wp.pose.pose.position.y - last.pose.pose.position.y
z_diff = wp.pose.pose.position.z - last.pose.pose.position.z
distance = math.sqrt(
pow(x_diff, 2) + pow(y_diff, 2) + pow(z_diff, 2))
velocity = math.sqrt(2 * self.breaking_acceleration *
max(0.0, distance - 5))
if velocity < 1.0:
velocity = 0.0
wp.twist.twist.linear.x = min(velocity,
wp.twist.twist.linear.x)
elif self.stop == 0:
for i in range(start_wp, start_wp + LOOKAHEAD_WPS):
j = i % len(self.base_waypoints.waypoints)
wp_new = Waypoint()
wp_new.pose.pose = self.base_waypoints.waypoints[j].pose.pose
wp_new.twist.twist.linear.x = self.base_waypoints.waypoints[
j].twist.twist.linear.x
self.final_waypoints.append(wp_new)
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closest to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
light = None
# List of positions that correspond to the line to stop in front of for
# a given intersection
stop_line_positions = self.config['stop_line_positions']
if (self.pose and self.waypoints and self.lights and self.camera_info):
light_waypoints = [Waypoint(pose=x.pose) for x in self.lights]
closest_idx = self.get_closest_waypoint(self.pose, light_waypoints)
closest_light = self.lights[closest_idx]
# if light is visible, we want to return the waypoint
if self.is_visible(self.pose, closest_light.pose.pose.position):
light = closest_light
# get stopping waypoint
x, y = stop_line_positions[closest_idx]
pose_stop = Pose(position=Point(x=x, y=y))
stamped = PoseStamped(pose=pose_stop, header=self.pose.header)
light_wp = self.get_closest_waypoint(
stamped, self.waypoints.waypoints)
if light:
state = self.get_light_state(light)
rospy.loginfo("Light state: {0}".format(state))
return light_wp, state
return -1, TrafficLight.UNKNOWN
def load_stop_line():
stop_line_yaml = '''
camera_info:
image_width: 800
image_height: 600
stop_line_positions:
- [1148.56, 1184.65]
- [1559.2, 1158.43]
- [2122.14, 1526.79]
- [2175.237, 1795.71]
- [1493.29, 2947.67]
- [821.96, 2905.8]
- [161.76, 2303.82]
- [351.84, 1574.65]
'''
import yaml
stop_lines = []
for l in yaml.load(stop_line_yaml)["stop_line_positions"]:
wp = Waypoint()
wp.pose.pose.position.x = l[0]
wp.pose.pose.position.y = l[1]
stop_lines.append(wp)
return stop_lines
def calc_waypoints(self, pose_msg, velocity_msg, traffic_waypoint_msg):
# Extract base_waypoints ahead of vehicle as a starting point for the final_waypoints.
first_idx = self.wp_search.get_closest_waypoint_idx_ahead(
pose_msg.pose.position.x, pose_msg.pose.position.y)
if self.previous_first_idx is None:
# Construct previous waypoint from the vehicles initial pose and velocity.
self.preceding_waypoint = Waypoint()
self.preceding_waypoint.pose.pose = pose_msg.pose
self.preceding_waypoint.twist.twist = velocity_msg.twist
self.preceding_waypoint.acceleration = 0.0
elif self.previous_first_idx != first_idx:
# Copy the preceding waypoint before updating the waypoint list.
self.preceding_waypoint = self.waypoints[
(first_idx - self.previous_first_idx - 1) %
len(self.base_waypoints_msg.waypoints)]
self.waypoints = self.base_waypoints_msg.waypoints[
first_idx:first_idx + LOOKAHEAD_WPS]
self.previous_first_idx = first_idx
# Calculate the final_waypoints
self.__accelerate_to_speed_limit()
self.__adjust_for_traffic_lights(first_idx, traffic_waypoint_msg)
self.__assert_speed_limit()
return self.waypoints
def decelerate_waypoints(self, waypoints, total_dist, closest_idx):
temp = []
cur_speed = self.twisted.twist.linear.x
stop_idx = self.stopline_wp_idx - 2 # two waypoints back from the line so front of car stops at the line
# total_dist = self.arc_distance(waypoints, 0, stop_idx - closest_idx)
# DEBUG
# rospy.loginfo("cur speed = {}".format(cur_speed))
# rospy.loginfo("closest_idx, stop_idx self.stopline_wp_idx total_dist = {} {} {} {} {}".format(closest_idx, stop_idx, self.stopline_wp_idx, closest_idx, total_dist))
for i, wp in enumerate(waypoints):
p = Waypoint()
p.pose = waypoints[i].pose
dist = self.arc_distance(waypoints, i, stop_idx - closest_idx)
if dist == 0:
vel = 0
else:
vel = dist / total_dist * cur_speed # linear ramp, fast to slower to zero, could use an S curve here
if vel < 1.:
vel = 0.
if i >= 1:
p.twist.twist.linear.x = vel
temp.append(p)
p = Waypoint()
p.pose = waypoints[len(waypoints) - 1].pose
p.twist.twist.linear.x = 0.
temp.append(p)
# DEBUG
# v_str = ""
# for j in range(len(temp)-1):
# v_str = v_str + "{00:.2f} ".format(temp[j].twist.twist.linear.x)
# rospy.loginfo("vels: {}\n".format(v_str))
return temp
def decelerate_waypoints(self, waypoints, start_idx, lookahead_wp):
# Calculate constant deceleration rate to stop the car (a = v**2 / (2*s))
stop_idx = lookahead_wp - self.traffic_stop_offset
dist = self.distance(waypoints, 0, stop_idx)
deceleration = self.setpoint_speed**2.0 / (2.0 * dist)
self.stop_trajectory = []
dist_remaining = dist
for i in range(0, stop_idx):
wp = waypoints[i]
# Create new wp instance, and keep pose of the original waypoints
p = Waypoint()
p.pose = wp.pose
# Modify speed by assuming constant deceleration until we stop (a = v^2 / (2*s))
vel = math.sqrt(max(2 * deceleration * dist_remaining, 0))
if vel < 1.0:
vel = 0
p.twist.twist.linear.x = vel
dist_remaining -= self.distance(waypoints, i, i + 1)
self.stop_trajectory.append(((start_idx + i) % self.num_of_wp, p))
for i in range(stop_idx, stop_idx + self.traffic_stop_offset):
wp = waypoints[i]
# Create new wp instance, and keep pose of the original waypoints
p = Waypoint()
p.pose = wp.pose
p.twist.twist.linear.x = 0
self.stop_trajectory.append(((start_idx + i) % self.num_of_wp, p))
return [x[1] for x in self.stop_trajectory]
return next_p[1], next_p[0]
# smoke test
if __name__ == '__main__':
import csv
import yaml
from sys import exit
fname = "../../../data/wp_yaw_const.csv"
waypoints = []
with open(fname) as wfile:
reader = csv.DictReader(wfile, ['x', 'y', 'z', 'yaw'])
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = tf.transformations.quaternion_from_euler(0., 0., float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = float(40*0.27778)
waypoints.append(p)
with open('sim_traffic_light_config.yaml') as f:
try:
config = yaml.load(f)
except yaml.YAMLError as exc:
print exc
exit()
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 get_forward_waypoints(self, new_closest_wp_idx, current_velocity, base_wps):
# Finds the defined number of waypoints ahead of the vehicle's current position along with their velocities
start_wp = new_closest_wp_idx
start_velocity = current_velocity.twist.linear.x
base_wps_count = len(base_wps)
end_wp = start_wp + LOOKAHEAD_WPS
if end_wp >= base_wps_count:
end_wp = end_wp - base_wps_count
end_velocity = base_wps[end_wp][3]
if (self.traffic_light_state != -1) and (start_wp <= self.traffic_light_state):
# rospy.loginfo("red light spotted at start wp, end wp = %s, %s, %s", start_wp, end_wp, self.traffic_light_state)
if self.traffic_light_state <= end_wp:
end_wp = self.traffic_light_state
end_velocity = 0
#rospy.loginfo("red light spotted at start wp, end wp, light wp = %s, %s", start_wp, end_wp)
num_of_way_points = end_wp - start_wp + 1
if num_of_way_points < 0:
num_of_way_points = num_of_way_points + base_wps_count
forward_wps = []
j = start_wp
v = current_velocity.twist.linear.x
for i in range(num_of_way_points):
wp = Waypoint()
wp.pose.header.frame_id = '/world'
wp.pose.header.stamp = rospy.Time.now()
wp.pose.pose.position.x = base_wps[j][0]
wp.pose.pose.position.y = base_wps[j][1]
wp.pose.pose.position.z = base_wps[j][2]
wp.twist.twist.linear.x = base_wps[j][3]
velocity_delta = base_wps[j][3] - v
if j == base_wps_count - 1:
dist_to_next_point = base_wps[0][4]
else:
dist_to_next_point = base_wps[j+1][4]
"""
if velocity_delta > 0:
max_accel = math.sqrt(ACCEL_LIMIT * dist_to_next_point)
#rospy.loginfo("max_accel = %s", max_accel)
if velocity_delta > max_accel:
wp.twist.twist.linear.x = v + max_accel
if velocity_delta < 0:
max_decel = math.sqrt(abs(DECEL_LIMIT) * dist_to_next_point) * 0.25
#rospy.loginfo("max_decel = %s", max_decel)
if velocity_delta < -max_decel:
wp.twist.twist.linear.x = v - max_decel
"""
# rospy.loginfo("checking velocity at x %s, velocity_delta, base_velocity, v = %s, %s, %s", wp.pose.pose.position.x, velocity_delta, base_wps[j][3], v)
velocity_step = 0.5
if velocity_delta > 0:
if velocity_delta > velocity_step:
wp.twist.twist.linear.x = v + velocity_step
if velocity_delta < 0:
if velocity_delta < -velocity_step:
wp.twist.twist.linear.x = v - velocity_step
# rospy.loginfo("setting velocity at x %s = %s", wp.pose.pose.position.x, wp.twist.twist.linear.x)
v = wp.twist.twist.linear.x
forward_wps.append(wp)
j = j + 1
if j >= base_wps_count:
j = 0
"""
velocity_step = 0.5
if end_velocity == 0:
#rospy.loginfo("stop light, len of forward_wps = %s", len(forward_wps))
end_velocity_chunk = 0
for i in range(len(forward_wps)):
if forward_wps[len(forward_wps)-1 - i].twist.twist.linear.x > end_velocity_chunk:
forward_wps[len(forward_wps)-1 - i].twist.twist.linear.x = end_velocity_chunk
end_velocity_chunk = end_velocity_chunk + velocity_step
#rospy.loginfo("slowdown velocity = %s", forward_wps[len(forward_wps)-1 - i].twist.twist.linear.x)
"""
if end_velocity == 0:
target_velocity = 0
for i in range(len(forward_wps)):
if forward_wps[len(forward_wps)-1 - i].twist.twist.linear.x > target_velocity:
forward_wps[len(forward_wps)-1 - i].twist.twist.linear.x = target_velocity
dist_from_prev_point = base_wps[end_wp - i][4]
target_velocity = target_velocity + (math.sqrt(abs(DECEL_LIMIT) * dist_from_prev_point) * 0.25)
#rospy.loginfo("slowing down wp, velocity = %s, %s", str(len(forward_wps)-1 - i), forward_wps[len(forward_wps)-1 - i].twist.twist.linear.x)
# rospy.loginfo("no_of_wp, start position, end position = %s, (%s,%s) (%s,%s)", len(forward_wps), forward_wps[0].pose.pose.position.x, forward_wps[0].pose.pose.position.y, forward_wps[-1].pose.pose.position.x, forward_wps[-1].pose.pose.position.y)
# rospy.loginfo("velocity_delta, start_velocity, end_velocity = %s, %s, %s", forward_wps[0].twist.twist.linear.x-current_velocity.twist.linear.x, forward_wps[0].twist.twist.linear.x, forward_wps[-1].twist.twist.linear.x)
#rospy.loginfo("count, start_wp, end_wp = %s, %s, %s", num_of_way_points, start_wp, end_wp)
return forward_wps
def waypoints_cb(self, msg):
# DONE: Implement
# make our own copy of the waypoints - they are static and do not change
global LOOKAHEAD_WPS
if self.waypoints is None:
# unsubscribe to the waypoint messages - cut down on resource usage
self.sub_waypoints.unregister()
self.sub_waypoints = None
# set the restricted speed limit
self.restricted_speed_in_mps = msg.waypoints[0].twist.twist.linear.x
# create our own copy of the waypoint array
self.waypoints = []
vptsd = []
dl = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2)
p0 = msg.waypoints[0].pose.pose.position
wpd = 0.
for waypoint in msg.waypoints:
# calculate distances
p1 = waypoint.pose.pose.position
ld = dl(p0, p1)
vptsd.append(wpd+ld)
p0 = p1
wpd += ld
# need to create a new waypoint array so not to overwrite old one
p = Waypoint()
p.pose.pose.position.x = waypoint.pose.pose.position.x
p.pose.pose.position.y = waypoint.pose.pose.position.y
p.pose.pose.position.z = waypoint.pose.pose.position.z
p.pose.pose.orientation.x = waypoint.pose.pose.orientation.x
p.pose.pose.orientation.y = waypoint.pose.pose.orientation.y
p.pose.pose.orientation.z = waypoint.pose.pose.orientation.z
p.pose.pose.orientation.w = waypoint.pose.pose.orientation.w
p.twist.twist.linear.x = waypoint.twist.twist.linear.x
p.twist.twist.linear.y = waypoint.twist.twist.linear.y
p.twist.twist.linear.z = waypoint.twist.twist.linear.z
p.twist.twist.angular.x = waypoint.twist.twist.angular.x
p.twist.twist.angular.y = waypoint.twist.twist.angular.y
p.twist.twist.angular.z = waypoint.twist.twist.angular.z
self.waypoints.append(p)
if LOOKAHEAD_WPS > len(msg.waypoints):
LOOKAHEAD_WPS = len(msg.waypoints)
# use the restricted speed limit in mps to create our deceleration points (in reverse)
wpx0 = [-LOOKAHEAD_WPS, 0., LOOKAHEAD_WPS]
wpy0 = [self.restricted_speed_in_mps, self.restricted_speed_in_mps, self.restricted_speed_in_mps]
poly0 = np.polyfit(np.array(wpx0), np.array(wpy0), 2)
self.cruzpoly = np.poly1d(poly0)
# since LOOKAHEAD_WPS is current set to 100 (meters 200*.5), a half is around 50 meters stopping distance
wpx1 = []
wpy1 = []
wpx1.append(-LOOKAHEAD_WPS)
wpy1.append(-0.1)
wpx1.append(0.)
wpy1.append(-0.2)
# 5 meters away
wpx1.append(5)
wpy1.append(MPS*.5)
wpx1.append(10)
wpy1.append(MPS*5)
wpx1.append(16)
wpy1.append(MPS*5)
# 2 seconds away
wpx1.append(max([self.restricted_speed_in_mps*2, 24]))
wpy1.append(max([self.restricted_speed_in_mps*.2, MPS*5]))
# 4 seconds away
wpx1.append(max([self.restricted_speed_in_mps*4, 45]))
wpy1.append(max([self.restricted_speed_in_mps*.3, MPS*6]))
# 6 seconds away
wpx1.append(max([self.restricted_speed_in_mps*6, 65]))
wpy1.append(max([self.restricted_speed_in_mps*.5, MPS*10]))
# 8 seconds away
wpx1.append(max([self.restricted_speed_in_mps*8, 85]))
wpy1.append(self.restricted_speed_in_mps)
wpx1.append(LOOKAHEAD_WPS)
wpy1.append(self.restricted_speed_in_mps)
poly1 = np.polyfit(np.array(wpx1), np.array(wpy1), 3)
self.decelpoly = np.poly1d(poly1)
# use the -0.01 speed to create our stop points
wpx2 = [-LOOKAHEAD_WPS, 0., LOOKAHEAD_WPS]
wpy2 = [-0.01, -0.01, -0.01]
poly2 = np.polyfit(np.array(wpx2), np.array(wpy2), 2)
self.stoppoly = np.poly1d(poly2)
wlen = len(self.waypoints)
velpolypoint = vptsd[wlen-1]
for i in range(LOOKAHEAD_WPS):
ideal_velocity = self.decelpoly([(velpolypoint-vptsd[wlen-1-LOOKAHEAD_WPS+i])])[0]
self.waypoints[wlen-1-LOOKAHEAD_WPS+i].twist.twist.linear.x = ideal_velocity
def getWaypoints(self, number):
self.final_waypoints = []
vptsx = []
vptsy = []
vptsd = []
wlen = len(self.waypoints)
# still initializing - don't move yet.
if self.state == INIT or self.cruzpoly is None or self.decelpoly is None:
# calculate normal trajectory
for i in range(len(vptsx)):
# need to create a new waypoint array so not to overwrite old one - don't move
p = Waypoint()
p.pose.pose.position.x = self.waypoints[self.cwp+i].pose.pose.position.x
p.pose.pose.position.y = self.waypoints[self.cwp+i].pose.pose.position.y
p.pose.pose.position.z = self.waypoints[self.cwp+i].pose.pose.position.z
p.pose.pose.orientation.x = self.waypoints[self.cwp+i].pose.pose.orientation.x
p.pose.pose.orientation.y = self.waypoints[self.cwp+i].pose.pose.orientation.y
p.pose.pose.orientation.z = self.waypoints[self.cwp+i].pose.pose.orientation.z
p.pose.pose.orientation.w = self.waypoints[self.cwp+i].pose.pose.orientation.w
p.twist.twist.linear.x = 0.
p.twist.twist.linear.y = 0.
p.twist.twist.linear.z = 0.
p.twist.twist.angular.x = 0.
p.twist.twist.angular.y = 0.
p.twist.twist.angular.z = 0.
self.final_waypoints.append(p)
velocity = 0
# GO!
else:
# change to local coordinates
dl = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2)
p0 = self.waypoints[self.cwp].pose.pose.position
wpd = 0.
for i in range(number):
x = self.waypoints[(self.cwp+i)%wlen].pose.pose.position.x
y = self.waypoints[(self.cwp+i)%wlen].pose.pose.position.y
lx, ly = self.getLocalXY(self.theta, x, y)
vptsx.append(lx)
vptsy.append(ly)
p1 = self.waypoints[(self.cwp+i)%wlen].pose.pose.position
ld = dl(p0, p1)
vptsd.append(wpd+ld)
p0 = p1
wpd += ld
# calculate cross track error (cte) for steering
steerpoly = np.polyfit(np.array(vptsx), np.array(vptsy), 3)
polynomial = np.poly1d(steerpoly)
# calculate cross track error (cte) for throttle/braking
tl_dist = self.distanceToTrafficLight()
# if green light and is less than 16 meters away - reset and GO!
if tl_dist is not None and tl_dist < 0. and tl_dist > -16.:
tl_dist = None
# if less than LOOKAHEAD_WPS points from the end of the tracks
if self.cwp > (wlen-LOOKAHEAD_WPS):
# calculate distance from last waypoint - minus five meters.
velpolypoint = self.distance(self.waypoints, self.cwp, wlen-2) - 5.
# already stopped within 5 meters of end waypoint and velocity is less than 1 MPS...
if velpolypoint < 5. and self.current_linear_velocity < MPS:
# calculate trajectory with stoppoly
self.cruz_control = self.stoppoly
else:
# calculate trajectory with decelpoly (remember its backwards!)
self.cruz_control = self.decelpoly
# if green traffic light but will take more than 2 seconds to reach it at current speed...
elif tl_dist is not None and tl_dist < 0. and self.current_linear_velocity > MPS*5 and -tl_dist > self.current_linear_velocity*2:
# calculate trajectory with decelpoly (remember its backwards!)
self.cruz_control = self.decelpoly
tl_dist = -tl_dist
velpolypoint = tl_dist
# if no traffic light or cannot stop in less than 16 meters and current velocity is over 6.5 Meters a second
elif tl_dist is None or (tl_dist < 16. and self.current_linear_velocity > 6.5*MPS):
# calculate trajectory with cruzpoly
self.cruz_control = self.cruzpoly
velpolypoint = 0.0
# already stopped within 5 meters of a traffic light and velocity is less than 1 MPS...
elif tl_dist is not None and tl_dist < 5. and self.current_linear_velocity < MPS:
# calculate trajectory with stoppoly
self.cruz_control = self.stoppoly
velpolypoint = 0.0
# traffic light coming near
else:
# calculate trajectory with decelpoly (remember its backwards!)
self.cruz_control = self.decelpoly
velpolypoint = tl_dist
# calculate trajectory
for i in range(len(vptsx)):
scte = polynomial([vptsx[i]])[0]
ideal_velocity = self.cruz_control([(velpolypoint-vptsd[i])])[0]
# we are off by more than 2 meters per second!
if self.current_linear_velocity > (ideal_velocity + 2.):
# make hard correction.
ideal_velocity *= 0.25
# need to create a new waypoint array so not to overwrite old one
p = Waypoint()
p.pose.pose.position.x = self.waypoints[(self.cwp+i)%wlen].pose.pose.position.x
p.pose.pose.position.y = self.waypoints[(self.cwp+i)%wlen].pose.pose.position.y
p.pose.pose.position.z = self.waypoints[(self.cwp+i)%wlen].pose.pose.position.z
p.pose.pose.orientation.x = self.waypoints[(self.cwp+i)%wlen].pose.pose.orientation.x
p.pose.pose.orientation.y = self.waypoints[(self.cwp+i)%wlen].pose.pose.orientation.y
p.pose.pose.orientation.z = self.waypoints[(self.cwp+i)%wlen].pose.pose.orientation.z
p.pose.pose.orientation.w = self.waypoints[(self.cwp+i)%wlen].pose.pose.orientation.w
p.twist.twist.linear.x = ideal_velocity
p.twist.twist.linear.y = 0.
p.twist.twist.linear.z = 0.
p.twist.twist.angular.x = 0.
p.twist.twist.angular.y = 0.
p.twist.twist.angular.z = scte
self.final_waypoints.append(p)
# set ideal velocity for current waypoint
velocity = self.cruz_control([(velpolypoint-vptsd[0])])[0]
print "state:", self.state, "current_linear_velocity:", self.current_linear_velocity, "velocity:", velocity, "redtlwp", self.redtlwp, "tl_dist:", tl_dist, "restricted_speed:", self.restricted_speed_in_mps
def waypoints_cb(self, msg):
"""
:param msg: Reading message from /base_waypoints publisher.
Published base_waypoints are stored in class variable self.waypoints.
:return: None
"""
# set the restricted speed limit
# Check to see if there are waypoints in the msg.
if self.waypoints is None:
# Ensure that list doesn't have anything assigned initially.
self.waypoints = []
self.restricted_speed_in_mps = msg.waypoints[
0].twist.twist.linear.x
vptsd = []
dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
(a.z - b.z)**2)
p0 = msg.waypoints[0].pose.pose.position
wpd = 0
# Loop the msg.waypoints
for waypoint in msg.waypoints:
# calculate distances
p1 = waypoint.pose.pose.position
ld = dl(p0, p1)
vptsd.append(wpd + ld)
p0 = p1
wpd += ld
# need to create a new waypoint array so not to overwrite old one
p = Waypoint()
p.pose.pose.position.x = waypoint.pose.pose.position.x
p.pose.pose.position.y = waypoint.pose.pose.position.y
p.pose.pose.position.z = waypoint.pose.pose.position.z
p.pose.pose.orientation.x = waypoint.pose.pose.orientation.x
p.pose.pose.orientation.y = waypoint.pose.pose.orientation.y
p.pose.pose.orientation.z = waypoint.pose.pose.orientation.z
p.pose.pose.orientation.w = waypoint.pose.pose.orientation.w
p.twist.twist.linear.x = waypoint.twist.twist.linear.x
p.twist.twist.linear.y = waypoint.twist.twist.linear.y
p.twist.twist.linear.z = waypoint.twist.twist.linear.z
p.twist.twist.angular.x = waypoint.twist.twist.angular.x
p.twist.twist.angular.y = waypoint.twist.twist.angular.y
p.twist.twist.angular.z = waypoint.twist.twist.angular.z
self.waypoints.append(p)
if self.LOOKAHEAD_WPS > len(msg.waypoints):
self.LOOKAHEAD_WPS = len(msg.waypoints)
# use the restricted speed limit in mps to create our deceleration points (in reverse)
wpx0 = [-self.LOOKAHEAD_WPS, 0, self.LOOKAHEAD_WPS]
wpy0 = [
2 * self.restricted_speed_in_mps,
2 * self.restricted_speed_in_mps,
2 * self.restricted_speed_in_mps
]
poly0 = np.polyfit(np.array(wpx0), np.array(wpy0), 2)
self.cruzpoly = np.poly1d(poly0)
# since LOOKAHEAD_WPS is current set to 100 (meters 200*.5), a half is around 50 meters stopping distance
wpx1 = []
wpy1 = []
wpx1.append(-self.LOOKAHEAD_WPS)
wpy1.append(-0.1)
wpx1.append(0.)
wpy1.append(-0.2)
# 5 meters away
wpx1.append(5)
wpy1.append(MPS * .5)
wpx1.append(10)
wpy1.append(MPS * 5)
wpx1.append(16)
wpy1.append(MPS * 5)
# 2 seconds away
wpx1.append(max([self.restricted_speed_in_mps * 2, 24]))
wpy1.append(max([self.restricted_speed_in_mps * .2, MPS * 5]))
# 4 seconds away
wpx1.append(max([self.restricted_speed_in_mps * 4, 45]))
wpy1.append(max([self.restricted_speed_in_mps * .3, MPS * 6]))
# 6 seconds away
wpx1.append(max([self.restricted_speed_in_mps * 6, 65]))
wpy1.append(max([self.restricted_speed_in_mps * .5, MPS * 10]))
# 8 seconds away
wpx1.append(max([self.restricted_speed_in_mps * 8, 85]))
wpy1.append(self.restricted_speed_in_mps)
wpx1.append(self.LOOKAHEAD_WPS)
wpy1.append(self.restricted_speed_in_mps)
poly1 = np.polyfit(np.array(wpx1), np.array(wpy1), 3)
self.decelpoly = np.poly1d(poly1)
# use the -0.01 speed to create our stop points
wpx2 = [-self.LOOKAHEAD_WPS, 0, self.LOOKAHEAD_WPS]
wpy2 = [-0.01, -0.01, -0.01]
poly2 = np.polyfit(np.array(wpx2), np.array(wpy2), 2)
self.stoppoly = np.poly1d(poly2)
wlen = len(self.waypoints)
velpolypoint = vptsd[wlen - 1]
for i in range(self.LOOKAHEAD_WPS):
ideal_velocity = self.decelpoly([
(velpolypoint - vptsd[wlen - 1 - self.LOOKAHEAD_WPS + i])
])[0]
self.waypoints[wlen - 1 - self.LOOKAHEAD_WPS +
i].twist.twist.linear.x = ideal_velocity
def getNextWaypoints(self, number):
"""
:param number: Number of waypoints, this populates final_waypoints
:return: None
"""
# Initializing variables
self.final_waypoints = []
vptsx = []
vptsy = []
vptsd = []
wlen = len(self.waypoints)
# still initializing - don't move yet.
velpolypoint = None
wpd = 0.
p0 = self.waypoints[self.currentWaypoints].pose.pose.position
dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
(a.z - b.z)**2)
for i in range(number):
x = self.waypoints[(self.currentWaypoints + i) %
wlen].pose.pose.position.x
y = self.waypoints[(self.currentWaypoints + i) %
wlen].pose.pose.position.y
lx, ly = self.getLocalXY(self.theta, x, y)
vptsx.append(lx)
vptsy.append(ly)
p1 = self.waypoints[(self.currentWaypoints + i) %
wlen].pose.pose.position
ld = dl(p0, p1)
vptsd.append(wpd + ld)
p0 = p1
wpd += ld
# calculate cross track error (cte) for steering
steerpoly = np.polyfit(np.array(vptsx), np.array(vptsy), 3)
polynomial = np.poly1d(steerpoly)
# calculate cross track error (cte) for throttle/braking
tl_dist = self.dist2TL()
#rospy.logwarn("Distance from Traffic Light is : {}".format(tl_dist))
# if green light and is less than 16 meters away - reset and GO!
if tl_dist is not None and tl_dist < 0. and tl_dist > -16.:
tl_dist = None
# if less than LOOKAHEAD_WPS points from the end of the tracks
if self.currentWaypoints > (wlen - self.LOOKAHEAD_WPS):
# calculate distance from last waypoint - minus five meters.
velpolypoint = self.distance(self.waypoints, self.currentWaypoints,
wlen - 2) - 5.
# already stopped within 5 meters of end waypoint and velocity is less than 1 MPS...
if velpolypoint < 5. and self.current_linear_velocity < MPS:
# calculate trajectory with stoppoly
self.cruz_control = self.stoppoly
else:
# calculate trajectory with decelpoly (remember its backwards!)
self.cruz_control = self.decelpoly
# if green traffic light but will take more than 2 seconds to reach it at current speed...
elif tl_dist is not None and tl_dist < 0. and self.current_linear_velocity > MPS * 5 and -tl_dist > self.current_linear_velocity * 2:
# calculate trajectory with decelpoly (remember its backwards!)
self.cruz_control = self.decelpoly
tl_dist = -tl_dist
velpolypoint = tl_dist
# if no traffic light or cannot stop in less than 16 meters and current velocity is over 6.5 Meters a second
elif tl_dist is None or (tl_dist < 16.
and self.current_linear_velocity > 6.5 * MPS):
# calculate trajectory with cruzpoly
self.cruz_control = self.cruzpoly
velpolypoint = 0.0
# already stopped within 5 meters of a traffic light and velocity is less than 1 MPS...
elif tl_dist is not None and tl_dist < 5. and self.current_linear_velocity < MPS:
# calculate trajectory with stoppoly
self.cruz_control = self.stoppoly
velpolypoint = 0.0
# traffic light coming near
else:
# calculate trajectory with decelpoly (remember its backwards!)
self.cruz_control = self.decelpoly
velpolypoint = tl_dist
# Calculating trajectory
for i in range(len(vptsx)):
scte = polynomial([vptsx[i]])[0]
ideal_velocity = self.cruz_control([(velpolypoint - vptsd[i])])[0]
# we are off by more than 2 meters per second!
if self.current_linear_velocity > (ideal_velocity + 2.):
# make hard correction.
ideal_velocity *= 0.25
p = Waypoint()
p.pose.pose.position.x = self.waypoints[(self.currentWaypoints + i)
%
wlen].pose.pose.position.x
p.pose.pose.position.y = self.waypoints[(self.currentWaypoints + i)
%
wlen].pose.pose.position.y
p.pose.pose.position.z = self.waypoints[(self.currentWaypoints + i)
%
wlen].pose.pose.position.z
p.pose.pose.orientation.x = self.waypoints[
(self.currentWaypoints + i) % wlen].pose.pose.orientation.x
p.pose.pose.orientation.y = self.waypoints[
(self.currentWaypoints + i) % wlen].pose.pose.orientation.y
p.pose.pose.orientation.z = self.waypoints[
(self.currentWaypoints + i) % wlen].pose.pose.orientation.z
p.pose.pose.orientation.w = self.waypoints[
(self.currentWaypoints + i) % wlen].pose.pose.orientation.w
p.twist.twist.linear.x = ideal_velocity
p.twist.twist.linear.y = 0.
p.twist.twist.linear.z = 0.
p.twist.twist.angular.x = 0.
p.twist.twist.angular.y = 0.
p.twist.twist.angular.z = scte
self.final_waypoints.append(p)
# set ideal velocity for current waypoint
velocity = self.cruz_control([(velpolypoint - vptsd[0])])[0]
rospy.loginfo("Car velocity: {}".format(velocity))
def decelerate(self, next_pt, next_tl, target_velocity=0.):
'''
Copy of the function used in waypoing loader to reduce
the velocity of the car when apporaching a tl
'''
if len(self.decel_wps) > 10:
if self.stopped == False:
return self.decel_wps[1:]
# else:
# rospy.loginfo("decel_wps removing element !!! stopped: %s", self.stopped)
# return self.decel_wps[1:]
# last = waypoints[-1]
# last.twist.twist.linear.x = target_velocity
# for wp in waypoints[:-1][::-1]:
# dist = self.distance(wp.pose.pose.position, last.pose.pose.position)
# vel = math.sqrt(2 * MAX_DECEL * dist)
# if vel < 1.:
# vel = 0.
# wp.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
xyz = self.wps[next_pt].pose.pose.position
q = self.wps[next_pt].pose.pose.orientation
end_xyz = self.wps[next_tl].pose.pose.position
end_q = self.wps[next_tl].pose.pose.orientation
(roll, pitch,
yaw) = tf.transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])
(endroll, endpitch,
end_yaw) = tf.transformations.euler_from_quaternion(
[end_q.x, end_q.y, end_q.z, end_q.w])
s, d = self.stopPlanner.getFrenet(xyz.x, xyz.y, yaw, self.wps)
#rospy.loginfo("tl_distance: %d, s: % %f" % self.tl_distance, s)
#dist_to_tl = self.stopPlanner.distance(self.wps, next_pt, next_tl)
ss = s + self.distance_to_tl
if self.current_velocity == 0:
current_velocity = 1.
else:
current_velocity = self.current_velocity
# T = 2. * dist_to_tl / current_velocity
dt = 0.03
if self.stopped == True:
#rospy.logwarn("current velocity = 0.")
#T = np.roots([0.5*MAX_ACCEL, 0.5, -(self.distance_to_tl)])
#T = T[T>0][0]
T = math.sqrt(2. * self.distance_to_tl)
# print("T: %f" % T)
n = T / dt
if n > LOOKAHEAD_WPS:
n = LOOKAHEAD_WPS
s_x = np.linspace(0, n * dt, n)
else:
s_x = np.linspace(0, T, n)
coeff = self.stopPlanner.JMT([s, 1.0, MAX_ACCEL], [ss, 0.0, 0.0],
T)
fy = np.poly1d(coeff)
sss = fy(s_x)
final_path = []
d = 0. ## we dont want to change lanes
for i in range(len(sss)):
px, py = self.stopPlanner.getXY(sss[i], d,
self.stopPlanner.map_s,
self.wps)
final_path.append([px, py])
#vvv.append(abs(sss[i]-sss[i+1])/ dt)
#px, py = self.stopPlanner.getXY(sss[-1], d, self.stopPlanner.map_s, self.wps)
#final_path.append([px, py])
final_path = np.array(final_path)
#vvv[1] = 1.0
#vvv = np.array(vvv)
vcoeff = self.stopPlanner.JMT([1.0, MAX_ACCEL, 1.0],
[0.0, 0.0, 0.0], T)
fyv = np.poly1d(vcoeff)
vvv = fyv(s_x)
vvv[vvv > self.velocity] = self.velocity
#vvv[vvv < 1.] = 1.0
vvv[vvv < 0.] = 0.0
#vvv[0] = 1.0
#vvv[1] = 2.0
# print(sss)
# print(vvv)
# print(pitch)
else:
#T = np.roots([0.5*MAX_DECEL, self.current_velocity, -(self.distance_to_tl)])
#T = T[T>0][0]
T = math.sqrt(2. * self.distance_to_tl)
# print("T: %f" % T)
n = T / dt
if n > LOOKAHEAD_WPS:
n = LOOKAHEAD_WPS
s_x = np.linspace(0, n * dt, n)
else:
s_x = np.linspace(0, T, n)
# print(s, self.current_velocity, MAX_DECEL, ss)
coeff = self.stopPlanner.JMT(
[s, self.current_velocity, -MAX_DECEL], [ss, 0.0, 0.0], T)
fy = np.poly1d(coeff)
sss = fy(s_x)
final_path = []
d = 0. ## we dont want to change lanes
#vvv = [self.current_velocity]
for i in range(len(sss)):
px, py = self.stopPlanner.getXY(sss[i], d,
self.stopPlanner.map_s,
self.wps)
final_path.append([px, py])
#vvv.append(abs(sss[i]-sss[i+1])/ dt)
#px, py = self.stopPlanner.getXY(sss[-1], d, self.stopPlanner.map_s, self.wps)
#final_path.append([px, py])
final_path = np.array(final_path)
#vvv = np.array(vvv)
vcoeff = self.stopPlanner.JMT(
[self.current_velocity, -MAX_DECEL, 1.0], [0.0, 0.0, 0.0], T)
fyv = np.poly1d(vcoeff)
vvv = fyv(s_x)
vvv[vvv > self.velocity] = self.velocity
#vvv[vvv < 1.0] = 0.0
#print(vvv)
o2lane = Lane()
o2lane.header.frame_id = '/world'
o2lane.header.stamp = rospy.Time(0)
cur_x = xyz.x
cur_y = xyz.y
waypoints = []
for i in range(len(final_path)):
p = Waypoint()
p.pose.pose.position.x = final_path[i][0]
p.pose.pose.position.y = final_path[i][1]
p.pose.pose.position.z = 0.
yw = math.atan2(final_path[i][1] - cur_y, final_path[i][0] - cur_x)
cur_x = final_path[i][0]
cur_y = final_path[i][1]
if yw < 0:
yw = yw + 2 * np.pi
q = tf.transformations.quaternion_from_euler(0., 0., yw)
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = vvv[i]
waypoints.append(p)
return waypoints
def get_lane(self, from_idx, to_idx, stop_idx=None):
lane = Lane()
lane.header = self.base_waypoints.header
lane.waypoints = [Waypoint()] * (to_idx - from_idx)
lane.waypoints = self.base_waypoints.waypoints[from_idx:to_idx]
return lane
def accelerate(self, next_pt, end_pt):
'''
Used by start_moving state but is not working yet
'''
if len(self.accel_wps) > 10:
return self.accel_wps[1:]
## JMT FULL
xyz = self.wps[next_pt].pose.pose.position
q = self.wps[next_pt].pose.pose.orientation
end_xyz = self.wps[end_pt].pose.pose.position
end_q = self.wps[end_pt].pose.pose.orientation
(roll, pitch,
yaw) = tf.transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])
(endroll, endpitch,
end_yaw) = tf.transformations.euler_from_quaternion(
[end_q.x, end_q.y, end_q.z, end_q.w])
## convert to frenet coordinates
s, d = self.stopPlanner.getFrenet(xyz.x, xyz.y, yaw, self.wps)
#rospy.loginfo("tl_distance: %d, s: % %f" % self.tl_distance, s)
if self.current_velocity < 1.:
start_velocity = 1.
else:
start_velocity = self.current_velocity
ss = s + self.stopPlanner.distance(self.wps, next_pt, end_pt)
dt = 0.03
# T = 3.0
# T = np.roots([0.5*MAX_ACCEL, start_velocity, -(ss - s)])
# T = T[T>0][0]
# print("T: %f" % T)
T = (self.velocity - start_velocity) / MAX_ACCEL
n = T / dt
if n > LOOKAHEAD_WPS:
n = LOOKAHEAD_WPS
s_x = np.linspace(0, n * dt, n)
else:
s_x = np.linspace(0, T, n)
coeff = self.stopPlanner.JMT([s, start_velocity, MAX_ACCEL],
[ss, self.velocity, MAX_ACCEL], T)
vcoeff = self.stopPlanner.JMT([start_velocity, MAX_ACCEL, 1.0],
[self.velocity, MAX_ACCEL, 1.0], T)
fy = np.poly1d(coeff)
fyv = np.poly1d(vcoeff)
# n = T / 0.03
# s_x = np.linspace(0, T, n)
sss = fy(s_x)
vvv = fyv(s_x)
vvv[vvv > self.velocity] = self.velocity
vvv[vvv < 1.] = 1.0
final_path = []
for i in range(len(sss)):
d = 0. ## we dont want to change lanes
px, py = self.stopPlanner.getXY(sss[i], d, self.stopPlanner.map_s,
self.wps)
final_path.append([px, py])
final_path = np.array(final_path)
o2lane = Lane()
o2lane.header.frame_id = '/world'
o2lane.header.stamp = rospy.Time(0)
cur_x = xyz.x
cur_y = xyz.y
waypoints = []
for i in range(len(final_path)):
p = Waypoint()
p.pose.pose.position.x = final_path[i][0]
p.pose.pose.position.y = final_path[i][1]
p.pose.pose.position.z = 0.
yw = math.atan2(final_path[i][1] - cur_y, final_path[i][0] - cur_x)
cur_x = final_path[i][0]
cur_y = final_path[i][1]
if yw < 0:
yw = yw + 2 * np.pi
q = tf.transformations.quaternion_from_euler(0., 0., yw)
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = vvv[i]
waypoints.append(p)
return waypoints
def decelerate_slow(self, next_pt, next_tl, target_velocity=0.):
'''
Copy of the function used in waypoing loader to reduce
the velocity of the car when apporaching a tl
'''
if len(self.decel_wps) > 10:
return self.decel_wps[1:]
xyz = self.wps[next_pt].pose.pose.position
q = self.wps[next_pt].pose.pose.orientation
end_xyz = self.wps[next_tl].pose.pose.position
end_q = self.wps[next_tl].pose.pose.orientation
(roll, pitch,
yaw) = tf.transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])
(endroll, endpitch,
end_yaw) = tf.transformations.euler_from_quaternion(
[end_q.x, end_q.y, end_q.z, end_q.w])
s, d = self.stopPlanner.getFrenet(xyz.x, xyz.y, yaw, self.wps)
#rospy.loginfo("tl_distance: %d, s: % %f" % self.tl_distance, s)
dist_to_tl = self.stopPlanner.distance(self.wps, next_pt, next_tl)
ss = s + dist_to_tl
dt = 0.03
a = (self.current_velocity**2 /
dist_to_tl) - (self.current_velocity**2 / (2. * dist_to_tl))
a = a * 3.
#T = np.roots([0.5*MAX_DECEL, self.current_velocity, -(distance_to_tl-self.stopped_cb)])
#T = T[T>0][0]
#T = self.current_velocity / MAX_DECEL
T = self.current_velocity / a
# print("T: %f" % T)
n = T / dt
if n > LOOKAHEAD_WPS:
n = LOOKAHEAD_WPS
s_x = np.linspace(0, n * dt, n)
else:
s_x = np.linspace(0, T, n)
# print(s, self.current_velocity, MAX_DECEL, ss)
coeff = self.stopPlanner.JMT([s, self.current_velocity, -a],
[ss, 0.0, 0.0], T)
fy = np.poly1d(coeff)
sss = fy(s_x)
final_path = []
d = 0. ## we dont want to change lanes
#vvv = [self.current_velocity]
for i in range(len(sss)):
px, py = self.stopPlanner.getXY(sss[i], d, self.stopPlanner.map_s,
self.wps)
final_path.append([px, py])
#vvv.append(abs(sss[i]-sss[i+1])/ dt)
#px, py = self.stopPlanner.getXY(sss[-1], d, self.stopPlanner.map_s, self.wps)
#final_path.append([px, py])
final_path = np.array(final_path)
#vvv = np.array(vvv)
vcoeff = self.stopPlanner.JMT([self.current_velocity, -a, 1.0],
[0.0, 0.0, 0.0], T)
fyv = np.poly1d(vcoeff)
vvv = fyv(s_x)
vvv[vvv > self.velocity] = self.velocity
vvv[vvv < 1.0] = 0.0
#print(vvv)
o2lane = Lane()
o2lane.header.frame_id = '/world'
o2lane.header.stamp = rospy.Time(0)
cur_x = xyz.x
cur_y = xyz.y
waypoints = []
for i in range(len(final_path)):
p = Waypoint()
p.pose.pose.position.x = final_path[i][0]
p.pose.pose.position.y = final_path[i][1]
p.pose.pose.position.z = 0.
yw = math.atan2(final_path[i][1] - cur_y, final_path[i][0] - cur_x)
cur_x = final_path[i][0]
cur_y = final_path[i][1]
if yw < 0:
yw = yw + 2 * np.pi
q = tf.transformations.quaternion_from_euler(0., 0., yw)
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = vvv[i]
waypoints.append(p)
return waypoints