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)