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))