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 _reset(self, car_pose):
camera_model_pose = self._get_initial_camera_pose(car_pose)
camera_model_state = ModelState()
camera_model_state.model_name = self.model_name
camera_model_state.pose = camera_model_pose
self.last_camera_state = camera_model_state
SetModelStateTracker.get_instance().set_model_state(camera_model_state)
def _reset(self, car_pose):
"""Reset camera position based on the track size"""
if self.is_reset_called:
return
# update camera position
model_pose = self._get_initial_camera_pose(car_pose)
camera_model_state = ModelState()
camera_model_state.model_name = self.model_name
camera_model_state.pose = model_pose
SetModelStateTracker.get_instance().set_model_state(camera_model_state)
def _reset_obstacles(self):
for obstacle_name, obstacle_pose in zip(self.obstacle_names, self.obstacle_poses):
obstacle_state = ModelState()
obstacle_state.model_name = obstacle_name
obstacle_state.pose = obstacle_pose
obstacle_state.twist.linear.x = 0
obstacle_state.twist.linear.y = 0
obstacle_state.twist.linear.z = 0
obstacle_state.twist.angular.x = 0
obstacle_state.twist.angular.y = 0
obstacle_state.twist.angular.z = 0
SetModelStateTracker.get_instance().set_model_state(obstacle_state)
def _update_bot_cars(self):
'''Update bot car objects locations
'''
self.bot_car_poses = self._compute_bot_car_poses()
for bot_car_name, bot_car_pose in zip(self.bot_car_names, self.bot_car_poses):
bot_car_state = ModelState()
bot_car_state.model_name = bot_car_name
bot_car_state.pose = bot_car_pose
bot_car_state.twist.linear.x = 0
bot_car_state.twist.linear.y = 0
bot_car_state.twist.linear.z = 0
bot_car_state.twist.angular.x = 0
bot_car_state.twist.angular.y = 0
bot_car_state.twist.angular.z = 0
SetModelStateTracker.get_instance().set_model_state(bot_car_state)
def _update(self, car_pose, delta_time):
# Calculate target Camera position based on car position
_, _, car_yaw = quaternion_to_euler(
x=car_pose.orientation.x,
y=car_pose.orientation.y,
z=car_pose.orientation.z,
w=car_pose.orientation.w,
)
# Linear Interpolate Yaw angle
car_yaw = utils.lerp_angle_rad(self.last_yaw, car_yaw,
delta_time * self.damping)
target_cam_quaternion = np.array(
euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=car_yaw))
target_camera_location = np.array([
car_pose.position.x, car_pose.position.y, 0.0
]) + apply_orientation(target_cam_quaternion,
np.array([-self.cam_dist_offset, 0, 0]))
target_camera_location_2d = target_camera_location[0:2]
# Linear interpolate Camera position to target position
cur_camera_2d_pos = np.array([
self.last_camera_state.pose.position.x,
self.last_camera_state.pose.position.y
])
new_cam_pos_2d = utils.lerp(cur_camera_2d_pos,
target_camera_location_2d,
delta_time * self.damping)
# Calculate camera rotation quaternion based on lookAt yaw
look_at_yaw = utils.get_angle_between_two_points_2d_rad(
self.last_camera_state.pose.position, car_pose.position)
cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad,
yaw=look_at_yaw)
# Configure Camera Model State
camera_model_state = ModelState()
camera_model_state.model_name = self.model_name
camera_model_state.pose.position.x = new_cam_pos_2d[0]
camera_model_state.pose.position.y = new_cam_pos_2d[1]
camera_model_state.pose.position.z = self.cam_fixed_height
camera_model_state.pose.orientation.x = cam_quaternion[0]
camera_model_state.pose.orientation.y = cam_quaternion[1]
camera_model_state.pose.orientation.z = cam_quaternion[2]
camera_model_state.pose.orientation.w = cam_quaternion[3]
SetModelStateTracker.get_instance().set_model_state(camera_model_state)
self.last_camera_state = camera_model_state
self.last_yaw = car_yaw
def _pause_car_model(self, should_reset_camera=False):
'''Pause agent immediately at the current position
'''
car_model_state = ModelState()
car_model_state.model_name = self._agent_name_
car_model_state.pose = self._pause_car_model_pose
car_model_state.twist.linear.x = 0
car_model_state.twist.linear.y = 0
car_model_state.twist.linear.z = 0
car_model_state.twist.angular.x = 0
car_model_state.twist.angular.y = 0
car_model_state.twist.angular.z = 0
SetModelStateTracker.get_instance().set_model_state(car_model_state)
if should_reset_camera:
self.camera_manager.reset(car_pose=car_model_state.pose,
namespace=self._agent_name_)
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 _park_car_model(self):
'''Park agent after racer complete F1 race
'''
car_model_state = ModelState()
car_model_state.model_name = self._agent_name_
orientation = euler_to_quaternion(yaw=0.0)
car_model_state.pose.position.x = self._park_position[0]
car_model_state.pose.position.y = self._park_position[1]
car_model_state.pose.position.z = 0.0
car_model_state.pose.orientation.x = orientation[0]
car_model_state.pose.orientation.y = orientation[1]
car_model_state.pose.orientation.z = orientation[2]
car_model_state.pose.orientation.w = orientation[3]
car_model_state.twist.linear.x = 0
car_model_state.twist.linear.y = 0
car_model_state.twist.linear.z = 0
car_model_state.twist.angular.x = 0
car_model_state.twist.angular.y = 0
car_model_state.twist.angular.z = 0
SetModelStateTracker.get_instance().set_model_state(car_model_state)
self.camera_manager.reset(car_pose=car_model_state.pose,
namespace=self._agent_name_)