def get_pose_at_time_list(self, trajectory_time_list):
if trajectory_time_list[-1] > self.time_horizon:
oclog.warn("try to get the pose out of the range of the path")
raise Exception("try to get the pose out of the range of the path")
time_list_in_segments = [[] for _ in range(len(self.time_deque))]
# split the distance list into several sublist with different segments
accumulated_time = 0
time_segment_index = 0
for i in range(len(self.time_deque)):
accumulated_time += self.time_deque[i][1]
# add all path distances that are less than accumulated length into current path segment list
while time_segment_index < len(trajectory_time_list) and \
trajectory_time_list[time_segment_index] <= accumulated_time:
time_list_in_segments[i].append(
trajectory_time_list[time_segment_index] -
accumulated_time + self.time_deque[i][1])
time_segment_index += 1
res = []
# get pose of each distance in different segments
for i in range(len(self.trajectory_deque)):
if len(time_list_in_segments[i]) > 0:
res.extend(self.trajectory_deque[i].get_pose_at_time_list([
time_list_in_segments[i][j] + self.time_deque[i][0]
for j in range(len(time_list_in_segments[i]))
]))
return res
def __init__(self, speed, accelerate_list, time_list):
if len(accelerate_list) != len(time_list):
oclog.warn(
"acceleration and time interval are not of the same length")
raise Exception(
"acceleration and time interval are not of the same length")
self.speed = speed
self.accelerate_list = accelerate_list
self.time_list = time_list
self.speed_profile_time = sum(self.time_list)
def get_speed_at_time(self, speed_profile_time):
"""
Get speed and acceleration of the dynamic object at time t
:param t: time
:return: [speed, acceleration]
"""
if speed_profile_time > self.speed_profile_time_horizon:
oclog.warn("Speed profile time exceeds total time")
raise Exception("Speed profile time exceeds total time")
return [
max(self.speed + self.acceleration * speed_profile_time, 0.0),
self.acceleration
]
def get_speed_at_time(self, speed_profile_time):
if speed_profile_time > self.speed_profile_time:
oclog.warn("Speed profile time exceeds total time")
raise Exception("Speed profile time exceeds total time")
current_speed = self.speed
current_time = speed_profile_time
index = 0
while current_time - self.time_list[index] > SMALL_NUMBER:
current_speed += self.accelerate_list[index] * self.time_list[index]
current_speed = max(current_speed, 0)
current_time -= self.time_list[index]
index += 1
current_speed += self.accelerate_list[index] * current_time
current_speed = max(current_speed, 0)
return [current_speed, self.accelerate_list[index]]
def __init__(self, path_segments):
"""
:param path_segments: a list of Curve objects which composes the path
"""
if path_segments is None:
oclog.warn("Path segment is None")
raise Exception('Path segment is None')
elif len(path_segments) == 0:
oclog.warn("Path with empty path segments created")
raise Exception('Path with empty path segments created')
self.path_segments = path_segments
self.path_length = 0.0
for path_segment in self.path_segments:
self.path_length += path_segment.get_length()
def get_pose_at_distance(self, path_distance):
if path_distance == self.get_length():
return self.path_segments[-1].get_pose_at_distance(
self.path_segments[-1].get_length())
elif path_distance > self.get_length():
oclog.warn(
"try to get the pose out of the range of the path: {}, {}. No exception is raised"
.format(path_distance, self.get_length()))
return self.path_segments[-1].get_pose_at_distance(
self.path_segments[-1].get_length())
for i in range(len(self.path_segments)):
if path_distance > self.path_segments[i].get_length():
path_distance -= self.path_segments[i].get_length()
else:
return self.path_segments[i].get_pose_at_distance(
path_distance)
def get_speed_at_time(self,
t,
maxacc=DEFAULT_MAX_ACCELERATION,
minacc=DEFAULT_MAX_DECELERATION):
"""
Get speed and acceleration of the dynamic object at time t
:param t: speed_profile_time
:return: [speed, acceleration]
"""
cur_quad_paras = self.cur_quad_paras
if t > self.speed_profile_time_horizon:
oclog.warn("Speed profile time exceeds total time")
raise Exception("Speed profile time exceeds total time")
return [(cur_quad_paras[0] * t**2 + cur_quad_paras[1] * t +
cur_quad_paras[2]),
(2 * cur_quad_paras[0] * t + cur_quad_paras[1])]
def get_pose_at_time(self, trajectory_time):
s = self.speed_profile.get_distance_at_time(trajectory_time)
if s < 0:
oclog.warn(
"trajectory distance is negative at input trajectory time")
oclog.warn("s: " + str(s))
oclog.warn("trajectory_time: " + str(trajectory_time))
oclog.warn("speed_profile.speed: " + str(self.speed_profile.speed))
raise Exception(
"trajectory distance is negative at input trajectory time")
p = self.path.get_pose_at_distance(s)
v = self.speed_profile.get_speed_at_time(trajectory_time)
p.speed = v[0]
p.acceleration = v[1]
p.jerk = 0
return p
def __init__(self, previous_trajectory, current_trajectory,
perception_gap_time, time_bound):
if not isinstance(previous_trajectory, PartialTrajectory):
# if previous trajectory is a single trajectory
if previous_trajectory.get_time_horizon(
) < perception_gap_time + time_bound:
oclog.warn(
"previous trajectory is not long enough to get partial trajectory!"
)
raise Exception(
"previous trajectory is not long enough to get partial trajectory!"
)
trajectory_deque = deque([previous_trajectory])
time_deque = deque([[perception_gap_time, time_bound]])
else:
if perception_gap_time == 0:
self.trajectory_deque = previous_trajectory.trajectory_deque
self.time_deque = previous_trajectory.time_deque
self.time_horizon = previous_trajectory.time_horizon
trajectory_deque = previous_trajectory.trajectory_deque
time_deque = previous_trajectory.time_deque
if len(trajectory_deque) == 0:
oclog.warn("previous trajectory is empty")
raise Exception("previous trajectory is empty")
while len(trajectory_deque) > 0:
if time_deque[0][1] <= perception_gap_time:
perception_gap_time -= time_deque[0][1]
trajectory_deque.popleft()
time_deque.popleft()
else:
time_deque[0][0] += perception_gap_time
time_deque[0][1] -= perception_gap_time
break
i = 0
while True:
if time_bound >= time_deque[i][1]:
time_bound -= time_deque[i][1]
i += 1
if i == len(time_deque) and time_bound > 0:
oclog.warn(
"previous trajectory is not long enough to get partial trajectory!"
)
raise Exception(
"previous trajectory is not long enough to get partial trajectory!"
)
else:
break
time_deque[i][1] = time_bound
trajectory_deque.append(current_trajectory)
time_deque.append([0, current_trajectory.get_time_horizon()])
self.trajectory_deque = trajectory_deque
self.time_deque = time_deque
self.time_horizon = 0
for i in range(len(self.time_deque)):
self.time_horizon += self.time_deque[i][1]
def get_distance_at_time(self, speed_profile_time):
"""
get the odometer of the dynamic object at time t
:param speed_profile_time: time
:return: odometer
"""
# ensure s >= 0:
if speed_profile_time > self.speed_profile_time_horizon:
oclog.warn("Speed profile time exceeds total time")
oclog.warn("access time: " + str(speed_profile_time))
oclog.warn("speed profile length: " +
str(self.speed_profile_time_horizon))
raise Exception("Speed profile time exceeds total time")
if self.acceleration < 0:
duration = min(speed_profile_time, -self.speed / self.acceleration)
else:
duration = speed_profile_time
return self.speed * duration + self.acceleration * duration**2 / 2.0
def get_pose_at_distance_list(self, path_distance_list):
"""
Find the segment for each path_distance's final position and retrieve pose in that segment.
Assume path_distance_list is in INCREASING order!
:param path_distance_list:
:return:
"""
if path_distance_list[-1] > self.get_length():
oclog.warn("max_distance: " + str(path_distance_list[-1]))
oclog.warn("distance_range: " + str(self.get_length()))
oclog.warn(
"try to get the pose out of the range of the path. No exception is raised"
)
# raise Exception("try to get the pose out of the range of the path")
distance_list_in_segments = [[]
for _ in range(len(self.path_segments))]
# split the distance list into several sublist with different segments
accumulated_length = 0
path_segment_index = 0
for i in range(len(self.path_segments)):
accumulated_length += self.path_segments[i].get_length()
# add all path distances that are less than accumulated length into current path segment list
while path_segment_index < len(path_distance_list) and \
path_distance_list[path_segment_index] <= accumulated_length:
distance_list_in_segments[i].append(
path_distance_list[path_segment_index] -
accumulated_length + self.path_segments[i].get_length())
path_segment_index += 1
res = []
# get pose of each distance in different segments
for i in range(len(self.path_segments)):
if len(distance_list_in_segments[i]) > 0:
res.extend(self.path_segments[i].get_pose_at_distance_list(
distance_list_in_segments[i]))
return res
def get_distance_at_time(self, speed_profile_time):
# if speed_profile_time > self.speed_profile_time:
# raise Exception("Speed profile time exceeds total time")
distance = 0.0
index = 0
current_speed = self.speed
current_time = speed_profile_time
while current_time - self.time_list[index] > SMALL_NUMBER:
if self.accelerate_list[index] < 0:
duration = min(self.time_list[index],
-current_speed / self.accelerate_list[index])
else:
duration = self.time_list[index]
distance += current_speed * duration + self.accelerate_list[
index] * duration**2 / 2.0
current_speed += self.accelerate_list[index] * duration
current_time -= self.time_list[index]
index += 1
if index == len(self.time_list):
oclog.warn(
"Time horizon is not enough. Use longer time horizon in config file."
)
oclog.warn("speed profile time: " + str(speed_profile_time))
oclog.warn("speed profile horizon: " +
str(self.speed_profile_time))
raise Exception(
"Time horizon is not enough. Use longer time horizon in config file."
)
if self.accelerate_list[index] < 0:
duration = min(current_time,
-current_speed / self.accelerate_list[index])
else:
duration = current_time
distance += current_speed * duration + self.accelerate_list[
index] * duration**2 / 2.0
return distance
def __init__(self, map_file_path):
self.map_info = HDMap(map_file_path)
if self.map_info.lane_size() == 0:
oclog.warn('A map must be given. Input does not have lanes.')
raise Exception('A map must be given. Input does not have lanes.')