def _reset_agent(self, reset_pos_type):
'''Reset agent to either starting pos or last pos
Args:
reset_pos_type (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)
start_model_state = self._get_car_reset_model_state(
reset_pos_type=reset_pos_type)
# This _reset_agent is called when either resuming from pause or restarting the episode.
# set_model_state and get_model_state is actually occurred asynchronously
# in tracker with simulation clock subscription. So, when the agent is
# resumed from pause and entering next step function call, either set_model_state
# or get_model_state may not actually happened and the agent position may be
# outdated. This can cause mis-judgement in crash reset behavior and re-enter
# pause state right after resuming.
# To avoid such case, use blocking to actually update the model position in gazebo
# and GetModelstateTracker to reflect the latest agent position right away when reset.
SetModelStateTracker.get_instance().set_model_state(start_model_state,
blocking=True)
GetModelStateTracker.get_instance().get_model_state(self._agent_name_,
'',
blocking=True)
# reset view cameras
self.camera_manager.reset(car_pose=start_model_state.pose,
namespace=self._agent_name_)
def reset_agent(self):
'''reset agent by reseting member variables, reset s3 metrics, and reset agent to
starting position at the beginning of each episode
'''
logger.info("Reset agent")
self._clear_data()
self._metrics.reset()
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0,
0.0)
start_model_state = self._get_car_start_model_state()
# set_model_state and get_model_state is actually occurred asynchronously
# in tracker with simulation clock subscription. So, when the agent is
# entering next step function call, either set_model_state
# or get_model_state may not actually happened and the agent position may be outdated.
# To avoid such case, use blocking to actually update the model position in gazebo
# and GetModelstateTracker to reflect the latest agent position right away when start.
SetModelStateTracker.get_instance().set_model_state(start_model_state,
blocking=True)
GetModelStateTracker.get_instance().get_model_state(self._agent_name_,
'',
blocking=True)
# reset view cameras
self.camera_manager.reset(car_pose=start_model_state.pose,
namespace=self._agent_name_)
self._track_data_.update_object_pose(self._agent_name_,
start_model_state.pose)
def wait_for_model(model_name, relative_entity_name):
model = GetModelStateTracker.get_instance().get_model_state(
model_name, relative_entity_name, blocking=True, auto_sync=False)
should_wait_for_model = not model.success
while should_wait_for_model:
time.sleep(WAIT_TO_PREVENT_SPAM)
model = GetModelStateTracker.get_instance().get_model_state(
model_name, relative_entity_name, blocking=True, auto_sync=False)
should_wait_for_model = not model.success
def wait_for_model(model_name, relative_entity_name):
"""wait for model to be ready and while not ready continuously waiting
Args:
model_name (str): wait model name
relative_entity_name (str): relative entity name to model name
"""
model = GetModelStateTracker.get_instance().get_model_state(
model_name, relative_entity_name, blocking=True, auto_sync=False)
should_wait_for_model = not model.success
while should_wait_for_model:
time.sleep(WAIT_TO_PREVENT_SPAM)
model = GetModelStateTracker.get_instance().get_model_state(
model_name, relative_entity_name, blocking=True, auto_sync=False)
should_wait_for_model = not model.success
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 update_tracker(self, delta_time, sim_time):
"""
Callback when sim time is updated
Args:
delta_time (float): time diff from last call
sim_time (Clock): simulation time
"""
with self.lock:
camera_name_space = copy.copy(self.camera_namespaces)
for namespace in camera_name_space:
car_model_state = GetModelStateTracker.get_instance().get_model_state(namespace, "")
self._update(car_pose=car_model_state.pose,
delta_time=delta_time,
namespace=namespace)
def _wait_for_spawn(self) -> None:
"""Wait for spawn to complete
"""
try_count = 0
while True:
msg = GetModelStateTracker.get_instance().get_model_state(
model_name=self._model_name,
relative_entity_name="",
blocking=True)
if msg.success:
rospy.loginfo("[AbsModel]: spawn {} completed".format(
self._model_name))
break
try_count += 1
if try_count > self._max_retry_attempts:
model_name = self._model_name
self._model_name = None
log_and_exit("[AbsModel]: spawn {} failed".format(model_name),
SIMAPP_SIMULATION_WORKER_EXCEPTION,
SIMAPP_EVENT_ERROR_CODE_500)
rospy.loginfo("[AbsModel]: model {} is in processing of "
"spawning".format(self._model_name))
time.sleep(self._backoff_time_sec)
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
'''
# get car state
car_model_state = GetModelStateTracker.get_instance().get_model_state(
self._agent_name_, '')
self._track_data_.update_object_pose(self._agent_name_,
car_model_state.pose)
link_states = [
GetLinkStateTracker.get_instance().get_link_state(
link_name, reference_frame).link_state
for link_name, reference_frame in zip(self._agent_link_name_list_,
self.reference_frames)
]
link_points = [
self.make_link_points(link_state) for link_state in link_states
]
current_car_pose = car_model_state.pose
try:
# Get the position of the agent
pos_dict = self._get_agent_pos(
current_car_pose, link_points,
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._data_dict_, action, self._json_actions_,
current_car_pose)
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
self._ctrl_status[AgentCtrlStatus.START_NDIST.
value] = self._data_dict_['start_ndist']
return {
self._agent_name_:
self._reset_rules_manager.update(self._ctrl_status)
}
