in src/markov/track_geom/track_data.py [0:0]
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))