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(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 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_observation_space(self): try: return get_observation_space(Input.CAMERA.value) except GenericError as ex: ex.log_except_and_exit(utils.SIMAPP_SIMULATION_WORKER_EXCEPTION) except Exception as ex: raise GenericRolloutException('{}'.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 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 _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 ''' 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 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_input_embedders(self, network_type): try: return get_front_camera_embedders(network_type) except GenericError as ex: ex.log_except_and_exit(utils.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.Lock() self.camera_namespaces = {} # there should be only one camera manager instance FrustumManager._instance_ = self
def __init__(self, name): if not name or not isinstance(name, str): raise GenericRolloutException( "Camera name cannot be None or empty string") self._name = name self._topic_name = self._name self.lock = threading.Lock() self.is_reset_called = False self.spawn_sdf_model = ServiceProxyWrapper(SPAWN_SDF_MODEL, SpawnModel) self.rospack = rospkg.RosPack()
def create_instance(camera_type, namespace=None, topic_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, topic_name=topic_name) elif camera_type == CameraType.TOP_CAMERA: return TopCamera(namespace=namespace, topic_name=topic_name) else: raise GenericRolloutException( "Unknown defined camera type: {}".format(camera_type))
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 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 get_class(camera_type): """ Factory method for getting camera class camera_type - String containing the desired camera type """ try: if isinstance(camera_type, str): camera_type = CameraType(camera_type) return CAMERA_TYPE_TO_CAMERA_CLASS_MAP[camera_type] except: raise GenericRolloutException("Unknown camera")
def __init__(self): if TopCamera._instance_ is not None: raise GenericRolloutException( "Attempting to construct multiple top video camera") super(TopCamera, self).__init__(TopCamera.name) rospy.wait_for_service('/gazebo/set_model_state') self.model_state_client = ServiceProxyWrapper( '/gazebo/set_model_state', SetModelState) self.track_data = TrackData.get_instance() # there should be only one top video camera TopCamera._instance_ = self
def add(self, reset_rule): '''Add a reset rule into manager Args: reset_rule (AbstractResetRule): composite reset rule class instance Raises: GenericRolloutException: ResetRule object passed is not an object of AbstractResetRule ''' if not isinstance(reset_rule, AbstractResetRule): raise GenericRolloutException( "ResetRule object passed is not an object of AbstractResetRule" ) self._reset_rules[reset_rule.name] = reset_rule
def update_agent(self, action): '''Update agent reward and metrics ater the action is taken Args: action (int): model metadata action_space index Returns: dict: dictionary of agent info with agent name as the key and info as the value Raises: GenericRolloutException: Cannot find position ''' try: # Get the position of the agent pos_dict = self._track_data_.get_agent_pos( self._agent_name_, self._agent_link_name_list_, const.RELATIVE_POSITION_OF_FRONT_OF_CAR) model_point = pos_dict[AgentPos.POINT.value] self._data_dict_['steps'] += 1 except Exception as ex: raise GenericRolloutException( 'Cannot find position: {}'.format(ex)) # Set the reward and training metrics set_reward_and_metrics(self._reward_params_, self._step_metrics_, self._agent_name_, pos_dict, self._track_data_, self._reverse_dir_, self._data_dict_, action, self._json_actions_) prev_pnt_dist = min( model_point.distance(self._prev_waypoints_['prev_point']), model_point.distance(self._prev_waypoints_['prev_point_2'])) self._data_dict_['current_progress'] = self._reward_params_[ const.RewardParam.PROG.value[0]] self._data_dict_['max_progress'] = max( self._data_dict_['max_progress'], self._data_dict_['current_progress']) self._prev_waypoints_['prev_point_2'] = self._prev_waypoints_[ 'prev_point'] self._prev_waypoints_['prev_point'] = model_point self._ctrl_status[AgentCtrlStatus.POS_DICT.value] = pos_dict self._ctrl_status[ AgentCtrlStatus.STEPS.value] = self._data_dict_['steps'] self._ctrl_status[AgentCtrlStatus.CURRENT_PROGRESS. value] = self._data_dict_['current_progress'] self._ctrl_status[AgentCtrlStatus.PREV_PROGRESS. value] = self._data_dict_['prev_progress'] self._ctrl_status[AgentCtrlStatus.PREV_PNT_DIST.value] = prev_pnt_dist return { self._agent_name_: self._reset_rules_manager.update(self._ctrl_status) }
def get_state(self, block=True): try: # Make sure the first image is the starting image image_data = self.image_buffer.get(block=block) # Read the image and resize to get the state image = Image.frombytes('RGB', (image_data.width, image_data.height), image_data.data, 'raw', 'RGB', 0, 1) image = image.resize(TRAINING_IMAGE_SIZE, resample=2) self.raw_data = image_data return {Input.CAMERA.value: np.array(image)} except utils.DoubleBuffer.Empty: return {} except Exception as ex: raise GenericRolloutException("Unable to set state: {}".format(ex))
def add(self, camera, namespace=None): """Add a camera to manage Args: camera (object): Camera object namespace (str): namespace """ if namespace == '*': raise GenericRolloutException( "* is not allowed to use as namespace name.") with self.lock: namespace = namespace or 'default' if namespace not in self.camera_namespaces: self.camera_namespaces[namespace] = set() self.camera_namespaces[namespace].add(camera)
def is_racecar_collided(self, racecar_wheel_points): '''Returns a true if there is a collision between the racecar and an object racecar_wheel_points - List of points that specifies the wheels of the training car ''' try: for object_pose, object_dims in zip(self.object_poses.values(), self.object_dims.values()): 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 True except Exception as ex: raise GenericRolloutException( "Unable to detect collision {}".format(ex))
def __init__(self): if FollowCarCamera._instance_ is not None: raise GenericRolloutException("Attempting to construct multiple follow car camera") super(FollowCarCamera, self).__init__(FollowCarCamera.name) rospy.wait_for_service('/gazebo/set_model_state') self.model_state_client = ServiceProxyWrapper('/gazebo/set_model_state', SetModelState) self.track_data = TrackData.get_instance() # Camera configuration constants self.look_down_angle_rad = math.pi / 6.0 # 30 degree self.cam_dist_offset = 1.2 self.cam_fixed_height = 1.0 self.damping = 1.0 # Camera states self.last_yaw = 0.0 self.last_camera_state = None # there should be only one video camera instance FollowCarCamera._instance_ = self
def create_sensor(sensor_type, config_dict): '''Factory method for creating sensors type - String containing the desired sensor type kwargs - Meta data, usually containing the topics to subscribe to, the concrete sensor classes are responsible for checking the topics. ''' if sensor_type == Input.CAMERA.value: return Camera() elif sensor_type == Input.LEFT_CAMERA.value: return LeftCamera() elif sensor_type == Input.STEREO.value: return DualCamera() elif sensor_type == Input.LIDAR.value: return Lidar() elif sensor_type == Input.OBSERVATION.value: return Observation() else: raise GenericRolloutException("Unknown sensor")
def get_distance_from_next_and_prev(self, model_point, prev_index, next_index): '''Returns a tuple, where the first value is the distance to the given previous points and the second value is the distance to given next point. model_point - Object returned by calling the model state service which contains the position data for the agent prev_index - Integer representing the index of the previous point next_index - Integer representing the index of the next point ''' try: dist_from_prev = model_point.distance( Point(self._center_line_.coords[prev_index])) dist_from_next = model_point.distance( Point(self._center_line_.coords[next_index])) return dist_from_prev, dist_from_next except Exception as ex: raise GenericRolloutException( "Unable to get distance to prev and next points: {}".format( ex))
def send_action(self, action): '''Publish action topic to gazebo to render Args: action (int): model metadata action_space index Raises: GenericRolloutException: Agent phase is not defined ''' if self._ctrl_status[AgentCtrlStatus.AGENT_PHASE.value] == AgentPhase.RUN.value: steering_angle = float(self._json_actions_[action]['steering_angle']) * math.pi / 180.0 speed = float(self._json_actions_[action]['speed'] / const.WHEEL_RADIUS) \ * self._speed_scale_factor_ send_action(self._velocity_pub_dict_, self._steering_pub_dict_, steering_angle, speed) elif self._ctrl_status[AgentCtrlStatus.AGENT_PHASE.value] == AgentPhase.PAUSE.value: send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0, 0.0) else: raise GenericRolloutException('Agent phase {} is not defined'.\ format(self._ctrl_status[AgentCtrlStatus.AGENT_PHASE.value]))
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 pahse is not defined ''' if self.bot_car_phase == AgentPhase.RUN.value: self.pause_end_time = self.current_sim_time for agent_name, _ in agents_info_map.items(): # check racecar crash with a bot_car crashed_object_name = agents_info_map[agent_name]\ [AgentInfo.CRASHED_OBJECT_NAME.value] \ if AgentInfo.CRASHED_OBJECT_NAME.value in agents_info_map[agent_name] else None if 'racecar' in agent_name and \ crashed_object_name and 'bot_car' in crashed_object_name: racecar_progress = agents_info_map[agent_name]\ [AgentInfo.CURRENT_PROGRESS.value] bot_car_progress = agents_info_map[crashed_object_name]\ [AgentInfo.CURRENT_PROGRESS.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_end_time += 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.current_sim_time > self.pause_end_time: 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 get_agent_pos(self, agent_name, link_name_list, 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. agent_name - String with the name of the agent link_name_list - List of strings containing the name of the links whose positions are to be retrieved. relative_pos - List containing the x-y relative position of the front of the agent ''' try: #Request the model state from gazebo model_state = self._get_model_state_(agent_name, '') #Compute the model's orientation model_orientation = np.array([ model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w ]) #Compute the model's location relative to the front of the agent model_location = np.array([model_state.pose.position.x, model_state.pose.position.y, model_state.pose.position.z]) + \ apply_orientation(model_orientation, np.array(relative_pos)) model_point = Point(model_location[0], model_location[1]) #Grab the location of the links make_link_points = lambda link: Point( link.link_state.pose.position.x, link.link_state.pose.position. y) link_points = [ make_link_points(self._get_link_state_(name, '')) for name in link_name_list ] return { AgentPos.ORIENTATION.value: model_orientation, AgentPos.POINT.value: model_point, AgentPos.LINK_POINTS.value: link_points } except Exception as ex: raise GenericRolloutException( "Unable to get position: {}".format(ex))
def configure_camera(namespaces=["racecar"]): global is_configure_camera_called with configure_camera_function_lock: if not is_configure_camera_called: is_configure_camera_called = True main_camera_type = rospy.get_param("MAIN_CAMERA", DEFAULT_MAIN_CAMERA) sub_camera_type = rospy.get_param("SUB_CAMERA", DEFAULT_SUB_CAMERA) main_camera = dict() for namespace in namespaces: main_camera[namespace] = CameraFactory.create_instance( camera_type=main_camera_type, topic_name="/{}/{}".format(namespace, "main_camera"), namespace=namespace) sub_camera = CameraFactory.create_instance( camera_type=sub_camera_type, topic_name="/{}".format("sub_camera"), namespace=namespace) return main_camera, sub_camera else: err_msg = "configure_camera called more than once. configure_camera MUST be called ONLY once!" raise GenericRolloutException(err_msg)
def get_nearest_points(self, model_point): '''Returns a dictionary with the keys specified in TrackNearPnts containing the nearest way points to the agent. model_point - Object returned by calling the model state service which contains the position data for the agent ''' try: near_pnt_ctr = \ self._center_line_.interpolate(self.get_norm_dist(model_point), normalized=True) near_pnt_in = \ self._inner_border_.interpolate(self._inner_border_.project(near_pnt_ctr)) near_pnt_out = \ self._outer_border_.interpolate(self._outer_border_.project(near_pnt_ctr)) return { TrackNearPnts.NEAR_PNT_CENT.value: near_pnt_ctr, TrackNearPnts.NEAR_PNT_IN.value: near_pnt_in, TrackNearPnts.NEAR_PNT_OUT.value: near_pnt_out } except Exception as ex: raise GenericRolloutException( "Unable to get nearest points: {}".format(ex))