def project_to_2d(point_on_plane, plane_center, plane_width, plane_height, plane_quaternion):
if not isinstance(point_on_plane, list) and not isinstance(point_on_plane, tuple) \
and not isinstance(point_on_plane, np.ndarray):
raise GenericRolloutException("point_on_plane must be a type of list, tuple, or numpy.ndarray")
if not isinstance(plane_center, list) and not isinstance(plane_center, tuple) \
and not isinstance(plane_center, np.ndarray):
raise GenericRolloutException("plane_center must be a type of list, tuple, or numpy.ndarray")
if not isinstance(plane_quaternion, list) and not isinstance(plane_quaternion, tuple) \
and not isinstance(plane_quaternion, np.ndarray):
raise GenericRolloutException("plane_quaternion must be a type of list, tuple, or numpy.ndarray")
point_on_plane = np.array(point_on_plane)
plane_center = np.array(plane_center)
plane_quaternion = np.array(plane_quaternion)
# Transpose the center back to origin
point_on_plane_from_origin = point_on_plane - plane_center
# Reverse the rotation so plane can align back to y-axis
inverse_cam_quaternion = inverse_quaternion(plane_quaternion)
point_on_y_axis = apply_orientation(inverse_cam_quaternion, point_on_plane_from_origin)
# Rotate pitch 90 degree and yaw 90 degree, so plane will align to x and y axis
# Remember rotation order is roll, pitch, yaw in euler_to_quaternion method
project_2d_quaternion = euler_to_quaternion(pitch=np.pi / 2.0, yaw=np.pi / 2.0)
point_on_2d_plane = apply_orientation(np.array(project_2d_quaternion), point_on_y_axis)
# Align plane to origin at x, y = (0, 0)
point_on_2d_plane = point_on_2d_plane + np.array([plane_width / 2.0, plane_height / 2.0, 0.0])
# Re-scale x and y space between 0 and 1
return (point_on_2d_plane[0] / plane_width), (point_on_2d_plane[1] / plane_height)
def update(self, agent_status):
'''Update reset rules done flag based on the agent status
Args:
agent_status (dict): dictionary contains the agent status
Returns:
dict: dictionary contains the agent info after desired action is taken
Raises:
GenericRolloutException: key is not found in AgentCtrlStatus enum
'''
try:
AgentCtrlStatus.validate_dict(agent_status)
except KeyError as ex:
raise GenericRolloutException("Key {}, not found".format(ex))
agent_info_map = {}
for reset_rule in self._reset_rules.values():
agent_info = reset_rule.update(agent_status)
agent_info_map.update(agent_info)
try:
AgentInfo.validate_dict(agent_info_map)
except KeyError as ex:
raise GenericRolloutException("Key {}, not found".format(ex))
return agent_info_map
def validate_input_observation_space(
self, input_observation_space: ObservationSpace):
if input_observation_space.shape[
0] % NUMBER_OF_LIDAR_VALUES_IN_EACH_SECTOR != 0:
raise GenericRolloutException(
"Number of total lidar values is not divisible by number of values in each sector"
)
def get_collided_object_name(self, racecar_name):
'''Get object name that racecar collide into
Args:
racecar_name (string): racecar name
Returns:
string: Crashed object name if there is a crashed object. Otherwise ''
Raises:
GenericRolloutException: Unable to detect collision
'''
try:
with self.noncollidable_object_lock:
noncollidable_objects = self.noncollidable_objects.copy()
racecar_pose = self.object_poses[racecar_name]
racecar_dims = self.object_dims[racecar_name]
object_bounding_points = TrackData.get_object_bounding_rect(
racecar_pose, racecar_dims)
for object_name in self.object_poses.keys():
if object_name != racecar_name and object_name not in noncollidable_objects:
object_pose = self.object_poses[object_name]
object_dims = self.object_dims[object_name]
object_boundary = Polygon(
TrackData.get_object_bounding_rect(
object_pose, object_dims))
if any([
object_boundary.contains(Point(p))
for p in object_bounding_points
]):
return object_name
return ''
except Exception as ex:
raise GenericRolloutException(
"Unable to detect collision {}".format(ex))
def test_visibility(self, point):
"""
Test the visibility of the point for each frustums defined.
(True indicates the point is in the frustum otherwise False)
Returns list of test results where the list size is same as number of frustums defined.
- This will be useful when multiple frustums are defined (like Stereo Camera) and want to find
which lens that object is visible.
point - 3d position
"""
with self.lock:
if not isinstance(point, list) and not isinstance(point, tuple) \
and not isinstance(point, np.ndarray):
raise GenericRolloutException(
"point must be a type of list, tuple, or numpy.ndarray")
target_pos = np.array(point)
frustum_tests = []
for frustum_planes in self.frustums:
is_in_frustum = True
for plane in frustum_planes:
plane_normal, plane_offset = plane
if np.dot(plane_normal, target_pos) - plane_offset <= 0.0:
is_in_frustum = False
break
frustum_tests.append(is_in_frustum)
return frustum_tests
def get_state(self, block=True):
try:
return {self.sensor_type: self.data_buffer.get(block=block)}
except utils.DoubleBuffer.Empty:
return {}
except Exception as ex:
raise GenericRolloutException("Unable to set state: {}".format(ex))
def _get_agent_pos(self, car_pose, car_link_points, relative_pos):
'''Returns a dictionary with the keys defined in AgentPos which contains
the position of the agent on the track, the location of the desired
links, and the orientation of the agent.
car_pose - Gazebo Pose of the agent
car_link_points (Point[]) - List of car's links' Points.
relative_pos - List containing the x-y relative position of the front of
the agent
'''
try:
# Compute the model's orientation
model_orientation = np.array([
car_pose.orientation.x, car_pose.orientation.y,
car_pose.orientation.z, car_pose.orientation.w
])
# Compute the model's location relative to the front of the agent
model_location = np.array([car_pose.position.x,
car_pose.position.y,
car_pose.position.z]) + \
apply_orientation(model_orientation, np.array(relative_pos))
model_point = Point(model_location[0], model_location[1])
return {
AgentPos.ORIENTATION.value: model_orientation,
AgentPos.POINT.value: model_point,
AgentPos.LINK_POINTS.value: car_link_points
}
except Exception as ex:
raise GenericRolloutException(
"Unable to get position: {}".format(ex))
def hide_visuals(self, visuals, ignore_keywords, retry_num=3):
"""
Set the transparencies to 1.0 for all links and visibles to False
to hide the model's visuals passed in.
Args:
visuals (Visuals): the visuals for the current model.
ignore_keywords (List): list of keywords name in visual which
we should not hidden its visual
retry_num (int): number of retries for hiding visuals.
"""
link_names = []
visual_names = []
for visual_name, link_name in zip(visuals.visual_names,
visuals.link_names):
is_to_hide = True
for name in ignore_keywords:
if name in visual_name:
is_to_hide = False
if is_to_hide:
visual_names.append(visual_name)
link_names.append(link_name)
retry_count = 0
while not self._are_visuals_hidden(link_names, visual_names):
if retry_count >= retry_num:
raise GenericRolloutException(
"Hide Visuals Gazebo Services constantly failing. Something Wrong with gzserver."
)
self._set_visible_false(link_names, visual_names)
self._set_transparent(link_names, visual_names)
retry_count += 1
time.sleep(2) # sleep 2 seconds until next retry
def get_input_embedders(self, network_type):
try:
return get_front_camera_embedders(network_type)
except GenericError as ex:
ex.log_except_and_exit(SIMAPP_SIMULATION_WORKER_EXCEPTION)
except Exception as ex:
raise GenericRolloutException('{}'.format(ex))
def get_observation_space(self):
try:
return get_observation_space(Input.CAMERA.value)
except GenericError as ex:
ex.log_except_and_exit(SIMAPP_SIMULATION_WORKER_EXCEPTION)
except Exception as ex:
raise GenericRolloutException('{}'.format(ex))
def get_collided_object_name(self, racecar_wheel_points, racecar_name):
"""Get object name that racecar collide into
Args:
racecar_wheel_points (list): List of points that specifies
the wheels of the training car
racecar_name (string): racecar name
Returns:
string: Crashed object name if there is a crashed object. Otherwise ''
Raises:
GenericRolloutException: Unable to detect collision
"""
try:
with self.noncollidable_object_lock:
noncollidable_objects = self.noncollidable_objects.copy()
for object_name in self.object_poses.keys():
if object_name != racecar_name and object_name not in noncollidable_objects:
object_pose = self.object_poses[object_name]
object_dims = self.object_dims[object_name]
object_boundary = Polygon(
TrackData.get_object_bounding_rect(
object_pose, object_dims))
if any([
object_boundary.contains(p)
for p in racecar_wheel_points
]):
return object_name
return ""
except Exception as ex:
raise GenericRolloutException(
"Unable to detect collision {}".format(ex))
def get_track_length(self):
"""Returns the length of the track"""
try:
return self.center_line.length
except Exception as ex:
raise GenericRolloutException(
"Unable to get track lenght: {}".format(ex))
def get_way_pnts(self):
"""Returns a list containing all the way points"""
try:
return list(self.center_line.coords)
except Exception as ex:
raise GenericRolloutException(
"Unable to get way points: {}".format(ex))
def __init__(self):
"""Initialize a ModelUpdater Object.
Raises:
GenericRolloutException: raise a GenericRolloutException if the object is no longer singleton.
"""
if ModelUpdater._instance_ is not None:
raise GenericRolloutException("Attempting to construct multiple ModelUpdater")
# Wait for required services to be available
rospy.wait_for_service(SET_MODEL_STATE)
# Wait for gazebo plugin services to be available
for service in GazeboServiceName:
if service.name in GAZEBO_SERVICES:
rospy.wait_for_service(service.value)
# Gazebo service that allows us to position the car
self._model_state_client = ServiceProxyWrapper(SET_MODEL_STATE, SetModelState)
self._get_model_prop = ServiceProxyWrapper(GazeboServiceName.GET_MODEL_PROPERTIES.value,
GetModelProperties)
self._get_visual_names = ServiceProxyWrapper(GazeboServiceName.GET_VISUAL_NAMES.value,
GetVisualNames)
self._get_visuals = ServiceProxyWrapper(GazeboServiceName.GET_VISUALS.value, GetVisuals)
self._set_visual_colors = ServiceProxyWrapper(GazeboServiceName.SET_VISUAL_COLORS.value,
SetVisualColors)
self._set_visual_visibles = ServiceProxyWrapper(GazeboServiceName.SET_VISUAL_VISIBLES.value,
SetVisualVisibles)
self._pause_physics = ServiceProxyWrapper(GazeboServiceName.PAUSE_PHYSICS.value, Empty)
self._unpause_physics = ServiceProxyWrapper(GazeboServiceName.UNPAUSE_PHYSICS.value, Empty)
self._set_model_state_tracker = SetModelStateTracker.get_instance()
self._get_model_state_tracker = GetModelStateTracker.get_instance()
# there should be only one model updater instance
ModelUpdater._instance_ = self
def get_input_embedders(self, network_type):
try:
return get_lidar_embedders(network_type, self.sensor_type)
except GenericError as ex:
ex.log_except_and_exit(SIMAPP_SIMULATION_WORKER_EXCEPTION)
except Exception as ex:
raise GenericRolloutException("{}".format(ex))
def _reset_agent(self, reset_pos):
'''Reset agent to either starting pos or last pos
Args:
reset_pos (string): start_pos/last_pos depending on reset
to starting position of the lap or position
from last frame
Raises:
GenericRolloutException: Reset position is not defined
'''
logger.info("Reset agent")
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0,
0.0)
if reset_pos == const.ResetPos.LAST_POS.value:
self._track_data_.car_ndist = self._data_dict_['current_progress']
start_dist = self._data_dict_['current_progress'] * \
self._track_data_.get_track_length() / 100.0
start_model_state = self._get_car_reset_model_state(start_dist)
elif reset_pos == const.ResetPos.START_POS.value:
self._track_data_.car_ndist = self._data_dict_['start_ndist']
start_dist = self._data_dict_[
'start_ndist'] * self._track_data_.get_track_length()
start_model_state = self._get_car_start_model_state(start_dist)
else:
raise GenericRolloutException(
'Reset position {} is not defined'.format(reset_pos))
self.set_model_state(start_model_state)
# reset view cameras
self.camera_manager.reset(start_model_state,
namespace=self._agent_name_)
def to_viewport_point(self, point):
with self.lock:
if not isinstance(point, list) and not isinstance(point, tuple) \
and not isinstance(point, np.ndarray):
raise GenericRolloutException("point must be a type of list, tuple, or numpy.ndarray")
ray_origin = np.array(point)
points_in_viewports = []
for cam_pose, near_plane_info in zip(self.cam_poses, self.near_plane_infos):
cam_pos = cam_pose["position"]
cam_quaternion = cam_pose["orientation"]
near_width = near_plane_info["width"]
near_height = near_plane_info["height"]
near_center = near_plane_info["position"]
near_normal = near_plane_info["normal"]
near_offset = near_plane_info["offset"]
ray_dir = normalize(cam_pos - ray_origin)
point_on_plane = ray_plane_intersect(ray_origin=ray_origin,
ray_dir=ray_dir,
plane_normal=near_normal,
plane_offset=near_offset,
threshold=self.threshold)
if point_on_plane is None:
points_in_viewports.append((-1.0, -1.0))
else:
point_in_viewport = project_to_2d(point_on_plane=point_on_plane,
plane_center=near_center,
plane_width=near_width,
plane_height=near_height,
plane_quaternion=cam_quaternion)
points_in_viewports.append(point_in_viewport)
return points_in_viewports
def get_observation_space(self):
try:
return get_observation_space(self.sensor_type, self.model_metadata)
except GenericError as ex:
ex.log_except_and_exit(SIMAPP_SIMULATION_WORKER_EXCEPTION)
except Exception as ex:
raise GenericRolloutException('{}'.format(ex))
def __init__(self):
if FrustumManager._instance_ is not None:
raise GenericRolloutException(
"Attempting to construct multiple frustum manager")
self.lock = threading.RLock()
self.frustum_map = {}
# there should be only one camera manager instance
FrustumManager._instance_ = self
def __init__(self):
if CameraManager._instance_ is not None:
raise GenericRolloutException("Attempting to construct multiple Camera Manager")
self.lock = threading.RLock()
self.camera_namespaces = {}
# there should be only one camera manager instance
CameraManager._instance_ = self
super(CameraManager, self).__init__()
def __init__(self):
if RandomizerManager._instance_ is not None:
raise GenericRolloutException(
"Attempting to construct multiple Randomizer Manager")
self.randomizers = []
# there should be only one randomizer manager instance
RandomizerManager._instance_ = self
def create_instance(camera_type, namespace=None, model_name=None):
"""
Factory method for creating camera instance
camera_type - Enum type or String containing the desired camera type
"""
try:
if isinstance(camera_type, str):
camera_type = CameraType(camera_type)
except:
raise GenericRolloutException(
"Unknown camera type: {}".format(camera_type))
if camera_type == CameraType.FOLLOW_CAR_CAMERA:
return FollowCarCamera(namespace=namespace, model_name=model_name)
elif camera_type == CameraType.TOP_CAMERA:
return TopCamera(namespace=namespace, model_name=model_name)
else:
raise GenericRolloutException(
"Unknown defined camera type: {}".format(camera_type))
def __init__(self):
if EffectManager._instance_ is not None:
raise GenericRolloutException(
"Attempting to construct multiple Effect Manager")
self.effects = set()
self.lock = threading.RLock()
# there should be only one randomizer manager instance
EffectManager._instance_ = self
super(EffectManager, self).__init__()
def get_norm_dist(self, model_point):
"""Returns the normalized position of the agent relative to the track
model_point - Object returned by calling the model state service which contains
the position data for the agent
"""
try:
return self.center_line.project(model_point, normalized=True)
except Exception as ex:
raise GenericRolloutException(
"Unable to get norm dist: {}".format(ex))
def __init__(self, name, namespace, model_name):
if not name or not isinstance(name, str):
raise GenericRolloutException("Camera name cannot be None or empty string")
self._name = name
self._model_name = model_name or self._name
self._namespace = namespace or "default"
self.lock = threading.Lock()
self.is_reset_called = False
self.spawn_sdf_model = ServiceProxyWrapper(SPAWN_SDF_MODEL, SpawnModel)
CameraManager.get_instance().add(self, namespace)
def points_on_track(self, points):
"""Returns a boolean list, where entries of true represent a point in the points list
being on the track, and values of false represent a point in the points list being
of the track.
points - List of points who will be checked for being on or off the track
"""
try:
return [self._road_poly_.contains(pnt) for pnt in points]
except Exception as ex:
raise GenericRolloutException(
"Unable to get points on track {}".format(ex))
def __init__(self):
if TrackerManager._instance_ is not None:
raise GenericRolloutException("Attempting to construct multiple TrackerManager")
self.priority_order = [consts.TrackerPriority.HIGH, consts.TrackerPriority.NORMAL, consts.TrackerPriority.LOW]
self.tracker_map = {}
for priority in self.priority_order:
self.tracker_map[priority] = set()
self.lock = threading.RLock()
self.last_time = 0.0
rospy.Subscriber('/clock', Clock, self._update_sim_time)
TrackerManager._instance_ = self
def __init__(self):
if SetModelStateTracker._instance_ is not None:
raise GenericRolloutException("Attempting to construct multiple SetModelState Tracker")
self.lock = threading.RLock()
self.model_state_map = {}
rospy.wait_for_service(SET_MODEL_STATES)
self.set_model_states = ServiceProxyWrapper(SET_MODEL_STATES, SetModelStates)
SetModelStateTracker._instance_ = self
super(SetModelStateTracker, self).__init__(priority=consts.TrackerPriority.LOW)
def judge_action(self, agents_info_map):
"""Judge action to see whether reset is needed
Args:
agents_info_map: Dictionary contains all agents info with agent name as the key
and info as the value
Returns:
tuple: None, None, None
Raises:
GenericRolloutException: bot car phase is not defined
"""
if self.bot_car_phase == AgentPhase.RUN.value:
self.pause_duration = 0.0
for agent_name, agent_info in agents_info_map.items():
if not self.track_data.is_object_collidable(agent_name):
continue
# check racecar crash with a bot_car
crashed_object_name = (
agent_info[AgentInfo.CRASHED_OBJECT_NAME.value] if
AgentInfo.CRASHED_OBJECT_NAME.value in agent_info else "")
# only trainable racecar agent has 'bot_car' as possible crashed object
if "bot_car" in crashed_object_name:
racecar_progress = get_normalized_progress(
agent_info[AgentInfo.CURRENT_PROGRESS.value],
start_ndist=agent_info[AgentInfo.START_NDIST.value],
)
bot_car_info = agents_info_map[crashed_object_name]
bot_car_progress = get_normalized_progress(
bot_car_info[AgentInfo.CURRENT_PROGRESS.value],
start_ndist=bot_car_info[AgentInfo.START_NDIST.value],
)
# transition to AgentPhase.PAUSE.value
if racecar_progress > bot_car_progress:
self.bot_cars_lane_change_end_times = [
t + self.penalty_seconds
for t in self.bot_cars_lane_change_end_times
]
self.bot_car_crash_count += 1
self.pause_duration += self.penalty_seconds
self.bot_car_phase = AgentPhase.PAUSE.value
break
elif self.bot_car_phase == AgentPhase.PAUSE.value:
# transition to AgentPhase.RUN.value
if self.pause_duration <= 0.0:
self.bot_car_phase = AgentPhase.RUN.value
else:
raise GenericRolloutException(
"bot car phase {} is not defined".format(self.bot_car_phase))
return None, None, None
def add(self, camera, namespace):
"""Add a camera to manage
Args:
camera (object): Camera object
namespace (str): namespace
"""
if not namespace or namespace == '*':
raise GenericRolloutException("namespace must be provided and '*' is not allowed.")
with self.lock:
if namespace not in self.camera_namespaces:
self.camera_namespaces[namespace] = set()
self.camera_namespaces[namespace].add(camera)
