def update(self, car_pose):
"""
Update the frustums
Args:
car_pose (Pose): Gazebo Pose of the agent
"""
with self.lock:
self.frustums = []
self.cam_poses = []
self.near_plane_infos = []
# Retrieve car pose to calculate camera pose.
car_pos = np.array([car_pose.position.x, car_pose.position.y,
car_pose.position.z])
car_quaternion = [car_pose.orientation.x,
car_pose.orientation.y,
car_pose.orientation.z,
car_pose.orientation.w]
for camera_offset in self.camera_offsets:
# Get camera position by applying position offset from the car position.
cam_pos = car_pos + apply_orientation(car_quaternion, camera_offset)
# Get camera rotation by applying car rotation and pitch angle of camera.
_, _, yaw = quaternion_to_euler(x=car_quaternion[0],
y=car_quaternion[1],
z=car_quaternion[2],
w=car_quaternion[3])
cam_quaternion = np.array(euler_to_quaternion(pitch=self.camera_pitch, yaw=yaw))
# Calculate frustum with camera position and rotation.
planes, cam_pose, near_plane_info = self._calculate_frustum_planes(cam_pos=cam_pos,
cam_quaternion=cam_quaternion)
self.frustums.append(planes)
self.cam_poses.append(cam_pose)
self.near_plane_infos.append(near_plane_info)
def _update_temp(self):
with self.lock:
self.frustums = []
self.cam_poses = []
self.near_plane_infos = []
car_model_state = self.model_state_client('racecar', '')
car_pos = np.array([
car_model_state.pose.position.x,
car_model_state.pose.position.y,
car_model_state.pose.position.z
])
car_quaternion = [
car_model_state.pose.orientation.x,
car_model_state.pose.orientation.y,
car_model_state.pose.orientation.z,
car_model_state.pose.orientation.w
]
for camera_offset in self.camera_offsets:
cam_pos = car_pos + apply_orientation(car_quaternion,
camera_offset)
_, _, yaw = quaternion_to_euler(x=car_quaternion[0],
y=car_quaternion[1],
z=car_quaternion[2],
w=car_quaternion[3])
cam_quaternion = np.array(
euler_to_quaternion(pitch=self.camera_pitch, yaw=yaw))
planes, cam_pose, near_plane_info = self._calculate_frustum_planes(
cam_pos=cam_pos, cam_quaternion=cam_quaternion)
self.frustums.append(planes)
self.cam_poses.append(cam_pose)
self.near_plane_infos.append(near_plane_info)
def _update(self, car_model_state, delta_time):
# Calculate target Camera position based on car position
_, _, car_yaw = quaternion_to_euler(
x=car_model_state.pose.orientation.x,
y=car_model_state.pose.orientation.y,
z=car_model_state.pose.orientation.z,
w=car_model_state.pose.orientation.w)
# Linear Interpolate Yaw angle
car_yaw = utils.lerp_angle_rad(self.last_yaw, car_yaw,
delta_time * self.damping)
target_cam_quaternion = np.array(
euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=car_yaw))
target_camera_location = np.array([car_model_state.pose.position.x,
car_model_state.pose.position.y,
0.0]) + \
apply_orientation(target_cam_quaternion, np.array([-self.cam_dist_offset, 0, 0]))
target_camera_location_2d = target_camera_location[0:2]
# Linear interpolate Camera position to target position
cur_camera_2d_pos = np.array([
self.last_camera_state.pose.position.x,
self.last_camera_state.pose.position.y
])
new_cam_pos_2d = utils.lerp(cur_camera_2d_pos,
target_camera_location_2d,
delta_time * self.damping)
# Calculate camera rotation quaternion based on lookAt yaw
look_at_yaw = utils.get_angle_between_two_points_2d_rad(
self.last_camera_state.pose.position,
car_model_state.pose.position)
cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad,
yaw=look_at_yaw)
# Configure Camera Model State
camera_model_state = ModelState()
camera_model_state.model_name = self.topic_name
camera_model_state.pose.position.x = new_cam_pos_2d[0]
camera_model_state.pose.position.y = new_cam_pos_2d[1]
camera_model_state.pose.position.z = self.cam_fixed_height
camera_model_state.pose.orientation.x = cam_quaternion[0]
camera_model_state.pose.orientation.y = cam_quaternion[1]
camera_model_state.pose.orientation.z = cam_quaternion[2]
camera_model_state.pose.orientation.w = cam_quaternion[3]
self.model_state_client(camera_model_state)
self.last_camera_state = camera_model_state
self.last_yaw = car_yaw
def _get_initial_camera_pose(self, car_model_state):
_, _, car_yaw = quaternion_to_euler(x=car_model_state.pose.orientation.x,
y=car_model_state.pose.orientation.y,
z=car_model_state.pose.orientation.z,
w=car_model_state.pose.orientation.w)
target_cam_quaternion = np.array(euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=car_yaw))
target_camera_location = np.array([car_model_state.pose.position.x,
car_model_state.pose.position.y,
0.0]) + \
apply_orientation(target_cam_quaternion, np.array([-self.cam_dist_offset, 0, 0]))
camera_model_pose = Pose()
camera_model_pose.position.x = target_camera_location[0]
camera_model_pose.position.y = target_camera_location[1]
camera_model_pose.position.z = self.cam_fixed_height
camera_model_pose.orientation.x = target_cam_quaternion[0]
camera_model_pose.orientation.y = target_cam_quaternion[1]
camera_model_pose.orientation.z = target_cam_quaternion[2]
camera_model_pose.orientation.w = target_cam_quaternion[3]
self.last_yaw = car_yaw
return camera_model_pose
def get_object_reward_params(self, racecar_name, model_point, car_pose):
"""Returns a dictionary with object-related reward function params."""
with self._lock_:
try:
object_locations = [[
pose.position.x, pose.position.y
] for name, pose in self.object_poses.items()
if racecar_name not in name]
object_poses = [
pose for name, pose in self.object_poses.items()
if racecar_name not in name
]
if not object_locations:
return {}
# Sort the object locations based on projected distance
num_objects = len(object_locations)
object_pdists = [
self.center_line.project(Point(p))
for p in object_locations
]
object_headings = [0.0] * num_objects
object_speeds = [0.0] * num_objects
if self._is_bot_car_:
for i, object_pose in enumerate(object_poses):
_, _, yaw = quaternion_to_euler(
x=object_pose.orientation.x,
y=object_pose.orientation.y,
z=object_pose.orientation.z,
w=object_pose.orientation.w,
)
object_headings[i] = yaw
object_speeds[i] = self._bot_car_speed_
# Find the prev/next objects
model_pdist = self.center_line.project(model_point)
object_order = np.argsort(object_pdists)
object_pdists_ordered = [
object_pdists[i] for i in object_order
]
prev_object_index, next_object_index = find_prev_next(
object_pdists_ordered, model_pdist)
prev_object_index = object_order[prev_object_index]
next_object_index = object_order[next_object_index]
# Figure out which one is the closest
object_points = [
Point([object_location[0], object_location[1]])
for object_location in object_locations
]
prev_object_point = object_points[prev_object_index]
next_object_point = object_points[next_object_index]
prev_object_dist = model_point.distance(prev_object_point)
next_object_dist = model_point.distance(next_object_point)
if prev_object_dist < next_object_dist:
closest_object_point = prev_object_point
else:
closest_object_point = next_object_point
# Figure out whether objects is left of center based on direction
objects_left_of_center = [
self._inner_poly_.contains(p) ^ (not self.is_ccw)
for p in object_points
]
# Get object distances to centerline
objects_distance_to_center = [
self.center_line.distance(p) for p in object_points
]
# Figure out if the next object is in the camera view
objects_in_camera = self.get_objects_in_camera_frustums(
agent_name=racecar_name, car_pose=car_pose)
is_next_object_in_camera = any(
object_in_camera_idx == next_object_index
for object_in_camera_idx, _ in objects_in_camera)
# Determine if they are in the same lane
return {
RewardParam.CLOSEST_OBJECTS.value[0]:
[prev_object_index, next_object_index],
RewardParam.OBJECT_LOCATIONS.value[0]:
object_locations,
RewardParam.OBJECTS_LEFT_OF_CENTER.value[0]:
objects_left_of_center,
RewardParam.OBJECT_SPEEDS.value[0]:
object_speeds,
RewardParam.OBJECT_HEADINGS.value[0]:
object_headings,
RewardParam.OBJECT_CENTER_DISTS.value[0]:
objects_distance_to_center,
RewardParam.OBJECT_CENTERLINE_PROJECTION_DISTANCES.value[0]:
object_pdists,
RewardParam.OBJECT_IN_CAMERA.value[0]:
is_next_object_in_camera,
}
except Exception as ex:
raise GenericRolloutException(
"Unable to get object reward params: {}".format(ex))
def get_object_reward_params(self, racecar_name, model_point, reverse_dir,
car_pose):
'''Returns a dictionary with object-related reward function params.'''
with self._lock_:
try:
object_locations = [[
pose.position.x, pose.position.y
] for name, pose in self.object_poses.items()
if racecar_name not in name]
object_poses = [
pose for name, pose in self.object_poses.items()
if racecar_name not in name
]
if not object_locations:
return {}
# Sort the object locations based on projected distance
object_pdists = [
self.center_line.project(Point(p))
for p in object_locations
]
object_order = np.argsort(object_pdists)
object_pdists = [object_pdists[i] for i in object_order]
object_locations = [object_locations[i] for i in object_order]
object_headings = []
object_speeds = []
object_headings = [0.0 for _ in object_order]
object_speeds = [0.0 for _ in object_order]
if self._is_bot_car_:
for i in object_order:
object_pose = object_poses[i]
_, _, yaw = quaternion_to_euler(
x=object_pose.orientation.x,
y=object_pose.orientation.y,
z=object_pose.orientation.z,
w=object_pose.orientation.w)
object_headings[i] = yaw
object_speeds[i] = self._bot_car_speed_
# Find the prev/next objects
model_pdist = self.center_line.project(model_point)
prev_object_index, next_object_index = find_prev_next(
object_pdists, model_pdist)
# Figure out which one is the closest
object_points = [
Point([object_location[0], object_location[1]])
for object_location in object_locations
]
prev_object_point = object_points[prev_object_index]
next_object_point = object_points[next_object_index]
prev_object_dist = model_point.distance(prev_object_point)
next_object_dist = model_point.distance(next_object_point)
if prev_object_dist < next_object_dist:
closest_object_point = prev_object_point
else:
closest_object_point = next_object_point
# Figure out whether objects is left of center based on direction
is_ccw = self._is_ccw_ ^ reverse_dir
objects_left_of_center = [self._inner_poly_.contains(p) ^ (not is_ccw) \
for p in object_points]
# Figure out which lane the model is in
model_nearest_pnts_dict = self.get_nearest_points(model_point)
model_nearest_dist_dict = self.get_nearest_dist(
model_nearest_pnts_dict, model_point)
model_is_inner = \
model_nearest_dist_dict[TrackNearDist.NEAR_DIST_IN.value] < \
model_nearest_dist_dict[TrackNearDist.NEAR_DIST_OUT.value]
# Figure out which lane the object is in
closest_object_nearest_pnts_dict = self.get_nearest_points(
closest_object_point)
closest_object_nearest_dist_dict = \
self.get_nearest_dist(closest_object_nearest_pnts_dict, closest_object_point)
closest_object_is_inner = \
closest_object_nearest_dist_dict[TrackNearDist.NEAR_DIST_IN.value] < \
closest_object_nearest_dist_dict[TrackNearDist.NEAR_DIST_OUT.value]
objects_in_camera = self.get_objects_in_camera_frustums(
agent_name=racecar_name,
car_pose=car_pose,
object_order=object_order)
is_next_object_in_camera = any(
object_in_camera_idx == next_object_index
for object_in_camera_idx, _ in objects_in_camera)
# Determine if they are in the same lane
return {
RewardParam.CLOSEST_OBJECTS.value[0]:
[prev_object_index, next_object_index],
RewardParam.OBJECT_LOCATIONS.value[0]:
object_locations,
RewardParam.OBJECTS_LEFT_OF_CENTER.value[0]:
objects_left_of_center,
RewardParam.OBJECT_SPEEDS.value[0]:
object_speeds,
RewardParam.OBJECT_HEADINGS.value[0]:
object_headings,
RewardParam.OBJECT_CENTERLINE_PROJECTION_DISTANCES.value[0]:
object_pdists,
RewardParam.OBJECT_IN_CAMERA.value[0]:
is_next_object_in_camera
}
except Exception as ex:
raise GenericRolloutException(
"Unable to get object reward params: {}".format(ex))