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 _spawn_cameras(self):
'''helper method for initializing cameras
'''
# virtual event configure camera does not need to wait for car to spawm because
# follow car camera is not tracking any car initially
camera_manager = CameraManager.get_instance()
# pop all camera under virtual event namespace
camera_manager.pop(namespace=VIRTUAL_EVENT)
# Spawn the follow car camera
LOG.info(
"[virtual event manager] Spawning virtual event follow car camera model"
)
initial_pose = self._track_data.get_racecar_start_pose(
racecar_idx=0,
racer_num=1,
start_position=get_start_positions(1)[0])
self._main_cameras[VIRTUAL_EVENT].spawn_model(
initial_pose,
os.path.join(self._deepracer_path, "models", "camera",
"model.sdf"))
LOG.info("[virtual event manager] Spawning sub camera model")
# Spawn the top camera model
self._sub_camera.spawn_model(
None,
os.path.join(self._deepracer_path, "models", "top_camera",
"model.sdf"))
def __init__(self, config_dict, run_phase_sink, metrics):
'''agent_name - String containing the name of the agent
config_dict - Dictionary containing all the keys in ConfigParams
run_phase_sink - Sink to recieve notification of a change in run phase
'''
# reset rules manager
self._metrics = metrics
self._is_continuous = config_dict[const.ConfigParams.IS_CONTINUOUS.value]
self._is_reset = False
self._pause_count = 0
self._reset_rules_manager = construct_reset_rules_manager(config_dict)
self._ctrl_status = dict()
self._ctrl_status[AgentCtrlStatus.AGENT_PHASE.value] = AgentPhase.RUN.value
self._config_dict = config_dict
self._number_of_resets = config_dict[const.ConfigParams.NUMBER_OF_RESETS.value]
self._off_track_penalty = config_dict[const.ConfigParams.OFF_TRACK_PENALTY.value]
self._collision_penalty = config_dict[const.ConfigParams.COLLISION_PENALTY.value]
self._pause_end_time = 0.0
self._reset_count = 0
# simapp_version speed scale
self._speed_scale_factor_ = get_speed_factor(config_dict[const.ConfigParams.VERSION.value])
# Store the name of the agent used to set agents position on the track
self._agent_name_ = config_dict[const.ConfigParams.AGENT_NAME.value]
# Store the name of the links in the agent, this should be const
self._agent_link_name_list_ = config_dict[const.ConfigParams.LINK_NAME_LIST.value]
# Store the reward function
self._reward_ = config_dict[const.ConfigParams.REWARD.value]
self._track_data_ = TrackData.get_instance()
# Create publishers for controlling the car
self._velocity_pub_dict_ = OrderedDict()
self._steering_pub_dict_ = OrderedDict()
for topic in config_dict[const.ConfigParams.VELOCITY_LIST.value]:
self._velocity_pub_dict_[topic] = rospy.Publisher(topic, Float64, queue_size=1)
for topic in config_dict[const.ConfigParams.STEERING_LIST.value]:
self._steering_pub_dict_[topic] = rospy.Publisher(topic, Float64, queue_size=1)
#Create default reward parameters
self._reward_params_ = const.RewardParam.make_default_param()
#Creat the default metrics dictionary
self._step_metrics_ = StepMetrics.make_default_metric()
# State variable to track if the car direction has been reversed
self._reverse_dir_ = False
# Dictionary of bools indicating starting position behavior
self._start_pos_behavior_ = \
{'change_start' : config_dict[const.ConfigParams.CHANGE_START.value],
'alternate_dir' : config_dict[const.ConfigParams.ALT_DIR.value]}
# Dictionary to track the previous way points
self._prev_waypoints_ = {'prev_point' : Point(0, 0), 'prev_point_2' : Point(0, 0)}
# Dictionary containing some of the data for the agent
self._data_dict_ = {'max_progress': 0.0,
'current_progress': 0.0,
'prev_progress': 0.0,
'steps': 0.0,
'start_ndist': 0.0}
#Load the action space
self._action_space_, self._json_actions_ = \
load_action_space(config_dict[const.ConfigParams.ACTION_SPACE_PATH.value])
#! TODO evaluate if this is the best way to reset the car
rospy.wait_for_service(SET_MODEL_STATE)
rospy.wait_for_service(GET_MODEL_STATE)
self.set_model_state = ServiceProxyWrapper(SET_MODEL_STATE, SetModelState)
self.get_model_client = ServiceProxyWrapper(GET_MODEL_STATE, GetModelState)
# Adding the reward data publisher
self.reward_data_pub = RewardDataPublisher(self._agent_name_, self._json_actions_)
# init time
self.last_time = 0.0
self.curr_time = 0.0
# subscriber to time to update camera position
self.camera_manager = CameraManager.get_instance()
# True if the agent is in the training phase
self._is_training_ = False
rospy.Subscriber('/clock', Clock, self._update_sim_time)
# Register to the phase sink
run_phase_sink.register(self)
# Make sure velicty and angle are set to 0
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0, 0.0)
start_pose = self._track_data_._center_line_.interpolate_pose(self._data_dict_['start_ndist'] * self._track_data_.get_track_length(),
reverse_dir=self._reverse_dir_,
finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
self._track_data_.initialize_object(self._agent_name_, start_pose, ObstacleDimensions.BOT_CAR_DIMENSION)
self.car_model_state = self.get_model_client(self._agent_name_, '')
self._reset_agent(reset_pos=const.ResetPos.START_POS.value)
def __init__(self, config_dict, run_phase_sink, metrics):
'''config_dict (dict): containing all the keys in ConfigParams
run_phase_sink (RunPhaseSubject): Sink to receive notification of a change in run phase
metrics (EvalMetrics/TrainingMetrics): Training or evaluation metrics
'''
# reset rules manager
self._metrics = metrics
self._is_continuous = config_dict[
const.ConfigParams.IS_CONTINUOUS.value]
self._reset_rules_manager = construct_reset_rules_manager(config_dict)
self._ctrl_status = dict()
self._ctrl_status[
AgentCtrlStatus.AGENT_PHASE.value] = AgentPhase.RUN.value
self._config_dict = config_dict
self._done_condition = config_dict.get(
const.ConfigParams.DONE_CONDITION.value, any)
self._number_of_resets = config_dict[
const.ConfigParams.NUMBER_OF_RESETS.value]
self._off_track_penalty = config_dict[
const.ConfigParams.OFF_TRACK_PENALTY.value]
self._collision_penalty = config_dict[
const.ConfigParams.COLLISION_PENALTY.value]
self._pause_duration = 0.0
self._reset_count = 0
self._curr_crashed_object_name = ''
# simapp_version speed scale
self._speed_scale_factor_ = get_speed_factor(
config_dict[const.ConfigParams.VERSION.value])
# Store the name of the agent used to set agents position on the track
self._agent_name_ = config_dict[const.ConfigParams.AGENT_NAME.value]
# Set start lane. This only support for two agents H2H race
self._agent_idx_ = get_racecar_idx(self._agent_name_)
# Get track data
self._track_data_ = TrackData.get_instance()
if self._agent_idx_ is not None:
self._start_lane_ = self._track_data_.inner_lane \
if self._agent_idx_ % 2 else self._track_data_.outer_lane
else:
self._start_lane_ = self._track_data_.center_line
# Store the name of the links in the agent, this should be const
self._agent_link_name_list_ = config_dict[
const.ConfigParams.LINK_NAME_LIST.value]
# Store the reward function
self._reward_ = config_dict[const.ConfigParams.REWARD.value]
# Create publishers for controlling the car
self._velocity_pub_dict_ = OrderedDict()
self._steering_pub_dict_ = OrderedDict()
for topic in config_dict[const.ConfigParams.VELOCITY_LIST.value]:
self._velocity_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in config_dict[const.ConfigParams.STEERING_LIST.value]:
self._steering_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
#Create default reward parameters
self._reward_params_ = const.RewardParam.make_default_param()
#Create the default metrics dictionary
self._step_metrics_ = StepMetrics.make_default_metric()
# Dictionary of bools indicating starting position behavior
self._start_pos_behavior_ = \
{'change_start' : config_dict[const.ConfigParams.CHANGE_START.value],
'alternate_dir' : config_dict[const.ConfigParams.ALT_DIR.value]}
# Dictionary to track the previous way points
self._prev_waypoints_ = {
'prev_point': Point(0, 0),
'prev_point_2': Point(0, 0)
}
# Normalized distance of new start line from the original start line of the track.
start_ndist = 0.0
# Normalized start position offset w.r.t to start_ndist, which is the start line of the track.
start_pos_offset = config_dict.get(
const.ConfigParams.START_POSITION.value, 0.0)
self._start_line_ndist_offset = start_pos_offset / self._track_data_.get_track_length(
)
# Dictionary containing some of the data for the agent
# - During the reset call, every value except start_ndist will get wiped out by self._clear_data
# (reset happens prior to every episodes begin)
# - If self._start_line_ndist_offset is not 0 (usually some minus value),
# then initial current_progress suppose to be non-zero (usually some minus value) as progress
# suppose to be based on start_ndist.
# - This will be correctly calculated by first call of utils.compute_current_prog function.
# As prev_progress will be initially 0.0 and physical position is not at start_ndist,
# utils.compute_current_prog will return negative progress if self._start_line_ndist_offset is negative value
# (meaning behind start line) and will return positive progress if self._start_line_ndist_offset is
# positive value (meaning ahead of start line).
self._data_dict_ = {
'max_progress': 0.0,
'current_progress': 0.0,
'prev_progress': 0.0,
'steps': 0.0,
'start_ndist': start_ndist,
'prev_car_pose': 0.0
}
#Load the action space
self._action_space_, self._json_actions_ = \
load_action_space(config_dict[const.ConfigParams.ACTION_SPACE_PATH.value])
#! TODO evaluate if this is the best way to reset the car
# Adding the reward data publisher
self.reward_data_pub = RewardDataPublisher(self._agent_name_,
self._json_actions_)
# subscriber to time to update camera position
self.camera_manager = CameraManager.get_instance()
# True if the agent is in the training phase
self._is_training_ = False
# Register to the phase sink
run_phase_sink.register(self)
# Make sure velocity and angle are set to 0
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0,
0.0)
# start_dist should be hypothetical start line (start_ndist) plus
# start position offset (start_line_ndist_offset).
start_pose = self._start_lane_.interpolate_pose(
(self._data_dict_['start_ndist'] + self._start_line_ndist_offset) *
self._track_data_.get_track_length(),
finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
self._track_data_.initialize_object(self._agent_name_, start_pose, \
ObstacleDimensions.BOT_CAR_DIMENSION)
self.make_link_points = lambda link_state: Point(
link_state.pose.position.x, link_state.pose.position.y)
self.reference_frames = ['' for _ in self._agent_link_name_list_]
self._pause_car_model_pose = None
self._park_position = DEFAULT_PARK_POSITION
AbstractTracker.__init__(self, TrackerPriority.HIGH)
def __init__(self,
queue_url,
aws_region='us-east-1',
race_duration=180,
number_of_trials=3,
number_of_resets=10000,
penalty_seconds=2.0,
off_track_penalty=2.0,
collision_penalty=5.0,
is_continuous=False,
race_type="TIME_TRIAL"):
# constructor arguments
self._model_updater = ModelUpdater.get_instance()
self._deepracer_path = rospkg.RosPack().get_path(
DeepRacerPackages.DEEPRACER_SIMULATION_ENVIRONMENT)
body_shell_path = os.path.join(self._deepracer_path, "meshes", "f1")
self._valid_body_shells = \
set(".".join(f.split(".")[:-1]) for f in os.listdir(body_shell_path) if os.path.isfile(
os.path.join(body_shell_path, f)))
self._valid_body_shells.add(const.BodyShellType.DEFAULT.value)
self._valid_car_colors = set(e.value for e in const.CarColorType
if "f1" not in e.value)
self._num_sectors = int(rospy.get_param("NUM_SECTORS", "3"))
self._queue_url = queue_url
self._region = aws_region
self._number_of_trials = number_of_trials
self._number_of_resets = number_of_resets
self._penalty_seconds = penalty_seconds
self._off_track_penalty = off_track_penalty
self._collision_penalty = collision_penalty
self._is_continuous = is_continuous
self._race_type = race_type
self._is_save_simtrace_enabled = False
self._is_save_mp4_enabled = False
self._is_event_end = False
self._done_condition = any
self._race_duration = race_duration
self._enable_domain_randomization = False
# sqs client
# The boto client errors out after polling for 1 hour.
self._sqs_client = SQSClient(queue_url=self._queue_url,
region_name=self._region,
max_num_of_msg=MAX_NUM_OF_SQS_MESSAGE,
wait_time_sec=SQS_WAIT_TIME_SEC,
session=refreshed_session(self._region))
self._s3_client = S3Client(region_name=self._region)
# tracking current state information
self._track_data = TrackData.get_instance()
self._start_lane = self._track_data.center_line
# keep track of the racer specific info, e.g. s3 locations, alias, car color etc.
self._current_racer = None
# keep track of the current race car we are using. It is always "racecar".
car_model_state = ModelState()
car_model_state.model_name = "racecar"
self._current_car_model_state = car_model_state
self._last_body_shell_type = None
self._last_sensors = None
self._racecar_model = AgentModel()
# keep track of the current control agent we are using
self._current_agent = None
# keep track of the current control graph manager
self._current_graph_manager = None
# Keep track of previous model's name
self._prev_model_name = None
self._hide_position_idx = 0
self._hide_positions = get_hide_positions(race_car_num=1)
self._run_phase_subject = RunPhaseSubject()
self._simtrace_video_s3_writers = []
self._local_model_directory = './checkpoint'
# virtual event only have single agent, so set agent_name to "agent"
self._agent_name = "agent"
# camera manager
self._camera_manager = CameraManager.get_instance()
# setting up virtual event top and follow camera in CameraManager
# virtual event configure camera does not need to wait for car to spawm because
# follow car camera is not tracking any car initially
self._main_cameras, self._sub_camera = configure_camera(
namespaces=[VIRTUAL_EVENT], is_wait_for_model=False)
self._spawn_cameras()
# pop out all cameras after configuration to prevent camera from moving
self._camera_manager.pop(namespace=VIRTUAL_EVENT)
dummy_metrics_s3_config = {
MetricsS3Keys.METRICS_BUCKET.value: "dummy-bucket",
MetricsS3Keys.METRICS_KEY.value: "dummy-key",
MetricsS3Keys.REGION.value: self._region
}
self._eval_metrics = EvalMetrics(
agent_name=self._agent_name,
s3_dict_metrics=dummy_metrics_s3_config,
is_continuous=self._is_continuous,
pause_time_before_start=PAUSE_TIME_BEFORE_START)
# upload a default best sector time for all sectors with time inf for each sector
# if there is not best sector time existed in s3
# use the s3 bucket and prefix for yaml file stored as environment variable because
# here is SimApp use only. For virtual event there is no s3 bucket and prefix past
# through yaml file. All are past through sqs. For simplicity, reuse the yaml s3 bucket
# and prefix environment variable.
virtual_event_best_sector_time = VirtualEventBestSectorTime(
bucket=os.environ.get("YAML_S3_BUCKET", ''),
s3_key=get_s3_key(os.environ.get("YAML_S3_PREFIX", ''),
SECTOR_TIME_S3_POSTFIX),
region_name=os.environ.get("APP_REGION", "us-east-1"),
local_path=SECTOR_TIME_LOCAL_PATH)
response = virtual_event_best_sector_time.list()
# this is used to handle situation such as robomaker job crash, so the next robomaker job
# can catch the best sector time left over from crashed job
if "Contents" not in response:
virtual_event_best_sector_time.persist(
body=json.dumps({
SECTOR_X_FORMAT.format(idx + 1): float("inf")
for idx in range(self._num_sectors)
}),
s3_kms_extra_args=utils.get_s3_kms_extra_args())
# ROS service to indicate all the robomaker markov packages are ready for consumption
signal_robomaker_markov_package_ready()
PhaseObserver('/agent/training_phase', self._run_phase_subject)
# setup mp4 services
self._setup_mp4_services()
def detach(self):
"""Detach camera from manager"""
CameraManager.get_instance().remove(self, self.namespace)
def __init__(self, config_dict, run_phase_sink, metrics):
'''agent_name - String containing the name of the agent
config_dict - Dictionary containing all the keys in ConfigParams
run_phase_sink - Sink to recieve notification of a change in run phase
'''
# reset rules manager
self._metrics = metrics
self._is_continuous = config_dict[
const.ConfigParams.IS_CONTINUOUS.value]
self._is_reset = False
self._reset_rules_manager = construct_reset_rules_manager(config_dict)
self._ctrl_status = dict()
self._ctrl_status[
AgentCtrlStatus.AGENT_PHASE.value] = AgentPhase.RUN.value
self._config_dict = config_dict
self._done_condition = config_dict.get(
const.ConfigParams.DONE_CONDITION.value, any)
self._number_of_resets = config_dict[
const.ConfigParams.NUMBER_OF_RESETS.value]
self._off_track_penalty = config_dict[
const.ConfigParams.OFF_TRACK_PENALTY.value]
self._collision_penalty = config_dict[
const.ConfigParams.COLLISION_PENALTY.value]
self._pause_duration = 0.0
self._reset_count = 0
self._curr_crashed_object_name = None
self._last_crashed_object_name = None
# simapp_version speed scale
self._speed_scale_factor_ = get_speed_factor(
config_dict[const.ConfigParams.VERSION.value])
# Store the name of the agent used to set agents position on the track
self._agent_name_ = config_dict[const.ConfigParams.AGENT_NAME.value]
# Set start lane. This only support for two agents H2H race
self._agent_idx_ = get_racecar_idx(self._agent_name_)
# Get track data
self._track_data_ = TrackData.get_instance()
if self._agent_idx_ is not None:
self._start_lane_ = self._track_data_.inner_lane \
if self._agent_idx_ % 2 else self._track_data_.outer_lane
else:
self._start_lane_ = self._track_data_.center_line
# Store the name of the links in the agent, this should be const
self._agent_link_name_list_ = config_dict[
const.ConfigParams.LINK_NAME_LIST.value]
# Store the reward function
self._reward_ = config_dict[const.ConfigParams.REWARD.value]
# Create publishers for controlling the car
self._velocity_pub_dict_ = OrderedDict()
self._steering_pub_dict_ = OrderedDict()
for topic in config_dict[const.ConfigParams.VELOCITY_LIST.value]:
self._velocity_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in config_dict[const.ConfigParams.STEERING_LIST.value]:
self._steering_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
#Create default reward parameters
self._reward_params_ = const.RewardParam.make_default_param()
#Creat the default metrics dictionary
self._step_metrics_ = StepMetrics.make_default_metric()
# Dictionary of bools indicating starting position behavior
self._start_pos_behavior_ = \
{'change_start' : config_dict[const.ConfigParams.CHANGE_START.value],
'alternate_dir' : config_dict[const.ConfigParams.ALT_DIR.value]}
# Dictionary to track the previous way points
self._prev_waypoints_ = {
'prev_point': Point(0, 0),
'prev_point_2': Point(0, 0)
}
# Normalize start position in meter to normalized start_ndist in percentage
start_ndist = config_dict.get(const.ConfigParams.START_POSITION.value, 0.0) / \
self._track_data_.get_track_length()
# Dictionary containing some of the data for the agent
self._data_dict_ = {
'max_progress': 0.0,
'current_progress': 0.0,
'prev_progress': 0.0,
'steps': 0.0,
'start_ndist': start_ndist
}
#Load the action space
self._action_space_, self._json_actions_ = \
load_action_space(config_dict[const.ConfigParams.ACTION_SPACE_PATH.value])
#! TODO evaluate if this is the best way to reset the car
# Adding the reward data publisher
self.reward_data_pub = RewardDataPublisher(self._agent_name_,
self._json_actions_)
# subscriber to time to update camera position
self.camera_manager = CameraManager.get_instance()
# True if the agent is in the training phase
self._is_training_ = False
# Register to the phase sink
run_phase_sink.register(self)
# Make sure velicty and angle are set to 0
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0,
0.0)
start_pose = self._track_data_.center_line.interpolate_pose(
self._data_dict_['start_ndist'] *
self._track_data_.get_track_length(),
finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
self._track_data_.initialize_object(self._agent_name_, start_pose, \
ObstacleDimensions.BOT_CAR_DIMENSION)
self.make_link_points = lambda link_state: Point(
link_state.pose.position.x, link_state.pose.position.y)
self.reference_frames = ['' for _ in self._agent_link_name_list_]
self._pause_car_model_pose = None
self._park_position = DEFAULT_PARK_POSITION
AbstractTracker.__init__(self, TrackerPriority.HIGH)
def __init__(self, config_dict):
'''agent_name - String containing the name of the agent
config_dict - Dictionary containing all the keys in ConfigParams
'''
# simapp_version speed scale
self._speed_scale_factor_ = get_speed_factor(
config_dict[const.ConfigParams.VERSION.value])
# Store the name of the agent used to set agents position on the track
self._agent_name_ = config_dict[const.ConfigParams.AGENT_NAME.value]
# Store the name of the links in the agent, this should be const
self._agent_link_name_list_ = config_dict[
const.ConfigParams.LINK_NAME_LIST.value]
# Store the reward function
self._reward_ = config_dict[const.ConfigParams.REWARD.value]
self._track_data_ = TrackData.get_instance()
# Create publishers for controlling the car
self._velocity_pub_dict_ = OrderedDict()
self._steering_pub_dict_ = OrderedDict()
for topic in config_dict[const.ConfigParams.VELOCITY_LIST.value]:
self._velocity_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in config_dict[const.ConfigParams.STEERING_LIST.value]:
self._steering_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
#Create default reward parameters
self._reward_params_ = const.RewardParam.make_default_param()
#Creat the default metrics dictionary
self._step_metrics_ = StepMetrics.make_default_metric()
# State variable to track if the car direction has been reversed
self._reverse_dir_ = False
# Dictionary of bools indicating starting position behavior
self._start_pos_behavior_ = \
{'change_start' : config_dict[const.ConfigParams.CHANGE_START.value],
'alternate_dir' : config_dict[const.ConfigParams.ALT_DIR.value]}
# Dictionary to track the previous way points
self._prev_waypoints_ = {
'prev_point': Point(0, 0),
'prev_point_2': Point(0, 0)
}
# Dictionary containing some of the data for the agent
self._data_dict_ = {
'prev_progress': 0.0,
'steps': 0.0,
'start_ndist': 0.0
}
#Load the action space
self._action_space_, self._json_actions_ = \
load_action_space(config_dict[const.ConfigParams.ACTION_SPACE_PATH.value])
#! TODO evaluate if this is the best way to reset the car
rospy.wait_for_service(SET_MODEL_STATE)
rospy.wait_for_service(GET_MODEL_STATE)
self.set_model_state = ServiceProxyWrapper(SET_MODEL_STATE,
SetModelState)
self.get_model_client = ServiceProxyWrapper(GET_MODEL_STATE,
GetModelState)
# Adding the reward data publisher
self.reward_data_pub = RewardDataPublisher(self._agent_name_,
self._json_actions_)
# init time
self.last_time = 0.0
self.curr_time = 0.0
# subscriber to time to update camera position
camera_types = [camera for camera in CameraType]
self.camera_manager = CameraManager(camera_types=camera_types)
rospy.Subscriber('/clock', Clock, self.update_camera)
# Make sure velicty and angle are set to 0
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0,
0.0)
class RolloutCtrl(AgentCtrlInterface):
'''Concrete class for an agent that drives forward'''
def __init__(self, config_dict):
'''agent_name - String containing the name of the agent
config_dict - Dictionary containing all the keys in ConfigParams
'''
# simapp_version speed scale
self._speed_scale_factor_ = get_speed_factor(
config_dict[const.ConfigParams.VERSION.value])
# Store the name of the agent used to set agents position on the track
self._agent_name_ = config_dict[const.ConfigParams.AGENT_NAME.value]
# Store the name of the links in the agent, this should be const
self._agent_link_name_list_ = config_dict[
const.ConfigParams.LINK_NAME_LIST.value]
# Store the reward function
self._reward_ = config_dict[const.ConfigParams.REWARD.value]
self._track_data_ = TrackData.get_instance()
# Create publishers for controlling the car
self._velocity_pub_dict_ = OrderedDict()
self._steering_pub_dict_ = OrderedDict()
for topic in config_dict[const.ConfigParams.VELOCITY_LIST.value]:
self._velocity_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in config_dict[const.ConfigParams.STEERING_LIST.value]:
self._steering_pub_dict_[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
#Create default reward parameters
self._reward_params_ = const.RewardParam.make_default_param()
#Creat the default metrics dictionary
self._step_metrics_ = StepMetrics.make_default_metric()
# State variable to track if the car direction has been reversed
self._reverse_dir_ = False
# Dictionary of bools indicating starting position behavior
self._start_pos_behavior_ = \
{'change_start' : config_dict[const.ConfigParams.CHANGE_START.value],
'alternate_dir' : config_dict[const.ConfigParams.ALT_DIR.value]}
# Dictionary to track the previous way points
self._prev_waypoints_ = {
'prev_point': Point(0, 0),
'prev_point_2': Point(0, 0)
}
# Dictionary containing some of the data for the agent
self._data_dict_ = {
'prev_progress': 0.0,
'steps': 0.0,
'start_ndist': 0.0
}
#Load the action space
self._action_space_, self._json_actions_ = \
load_action_space(config_dict[const.ConfigParams.ACTION_SPACE_PATH.value])
#! TODO evaluate if this is the best way to reset the car
rospy.wait_for_service(SET_MODEL_STATE)
rospy.wait_for_service(GET_MODEL_STATE)
self.set_model_state = ServiceProxyWrapper(SET_MODEL_STATE,
SetModelState)
self.get_model_client = ServiceProxyWrapper(GET_MODEL_STATE,
GetModelState)
# Adding the reward data publisher
self.reward_data_pub = RewardDataPublisher(self._agent_name_,
self._json_actions_)
# init time
self.last_time = 0.0
self.curr_time = 0.0
# subscriber to time to update camera position
camera_types = [camera for camera in CameraType]
self.camera_manager = CameraManager(camera_types=camera_types)
rospy.Subscriber('/clock', Clock, self.update_camera)
# Make sure velicty and angle are set to 0
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0,
0.0)
def update_camera(self, sim_time):
# get car state
car_model_state = self.get_model_client('racecar', '')
# update timer
self.curr_time = sim_time.clock.secs + 1.e-9 * sim_time.clock.nsecs
time_delta = self.curr_time - self.last_time
self.last_time = self.curr_time
# update camera pose
self.camera_manager.update(car_model_state, time_delta)
@property
def action_space(self):
return self._action_space_
def reset_agent(self):
send_action(self._velocity_pub_dict_, self._steering_pub_dict_, 0.0,
0.0)
self._track_data_.car_ndist = self._data_dict_[
'start_ndist'] # TODO -- REMOVE THIS
# Compute the start pose
start_dist = self._data_dict_[
'start_ndist'] * self._track_data_.get_track_length()
start_pose = self._track_data_._center_line_.interpolate_pose(
start_dist,
reverse_dir=self._reverse_dir_,
finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
# If we have obstacles, don't start near one
for object_pose in self._track_data_.object_poses.values():
object_point = Point(
[object_pose.position.x, object_pose.position.y])
object_dist = self._track_data_._center_line_.project(object_point)
object_dist_ahead = (object_dist - start_dist
) % self._track_data_.get_track_length()
object_dist_behind = (start_dist - object_dist
) % self._track_data_.get_track_length()
if self._reverse_dir_:
object_dist_ahead, object_dist_behind = object_dist_behind, object_dist_ahead
if object_dist_ahead < 1.0 or object_dist_behind < 0.5: # TODO: don't hard-code these numbers
object_nearest_pnts_dict = self._track_data_.get_nearest_points(
object_point)
object_nearest_dist_dict = self._track_data_.get_nearest_dist(
object_nearest_pnts_dict, object_point)
object_is_inner = object_nearest_dist_dict[TrackNearDist.NEAR_DIST_IN.value] < \
object_nearest_dist_dict[TrackNearDist.NEAR_DIST_OUT.value]
if object_is_inner:
start_pose = self._track_data_._outer_lane_.interpolate_pose(
self._track_data_._outer_lane_.project(
Point(start_pose.position.x,
start_pose.position.y)),
reverse_dir=self._reverse_dir_,
finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
else:
start_pose = self._track_data_._inner_lane_.interpolate_pose(
self._track_data_._inner_lane_.project(
Point(start_pose.position.x,
start_pose.position.y)),
reverse_dir=self._reverse_dir_,
finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
break
start_state = ModelState()
start_state.model_name = self._agent_name_
start_state.pose = start_pose
start_state.twist.linear.x = 0
start_state.twist.linear.y = 0
start_state.twist.linear.z = 0
start_state.twist.angular.x = 0
start_state.twist.angular.y = 0
start_state.twist.angular.z = 0
self.set_model_state(start_state)
# reset view cameras
self.camera_manager.reset(start_state)
def send_action(self, action):
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)
def judge_action(self, action):
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]
# Compute the next index
current_ndist = self._track_data_.get_norm_dist(model_point)
prev_index, next_index = self._track_data_.find_prev_next_waypoints(
current_ndist, normalized=True, reverse_dir=self._reverse_dir_)
# Set the basic reward and training metrics
set_reward_and_metrics(self._reward_params_, self._step_metrics_,
pos_dict, self._track_data_, next_index,
prev_index, action, self._json_actions_)
# Convert current progress to be [0,100] starting at the initial waypoint
if self._reverse_dir_:
self._reward_params_[const.RewardParam.LEFT_CENT.value[0]] = \
not self._reward_params_[const.RewardParam.LEFT_CENT.value[0]]
current_progress = self._data_dict_[
'start_ndist'] - current_ndist
else:
current_progress = current_ndist - self._data_dict_[
'start_ndist']
current_progress = compute_current_prog(
current_progress, self._data_dict_['prev_progress'])
self._data_dict_['steps'] += 1
# Add the agen specific metrics
self._step_metrics_[StepMetrics.STEPS.value] = \
self._reward_params_[const.RewardParam.STEPS.value[0]] = self._data_dict_['steps']
self._reward_params_[
const.RewardParam.REVERSE.value[0]] = self._reverse_dir_
self._step_metrics_[StepMetrics.PROG.value] = \
self._reward_params_[const.RewardParam.PROG.value[0]] = current_progress
except Exception as ex:
raise GenericRolloutException(
'Cannot find position: {}'.format(ex))
# This code should be replaced with the contact sensor code
is_crashed = False
model_heading = self._reward_params_[
const.RewardParam.HEADING.value[0]]
obstacle_reward_params = self._track_data_.get_object_reward_params(
model_point, model_heading, current_progress, self._reverse_dir_)
if obstacle_reward_params:
self._reward_params_.update(obstacle_reward_params)
is_crashed = self._track_data_.is_racecar_collided(
pos_dict[AgentPos.LINK_POINTS.value])
prev_pnt_dist = min(
model_point.distance(self._prev_waypoints_['prev_point']),
model_point.distance(self._prev_waypoints_['prev_point_2']))
is_off_track = not any(
self._track_data_.points_on_track(
pos_dict[AgentPos.LINK_POINTS.value]))
is_immobilized = (prev_pnt_dist <= 0.0001
and self._data_dict_['steps'] %
const.NUM_STEPS_TO_CHECK_STUCK == 0)
is_lap_complete = current_progress >= 100.0
self._reward_params_[const.RewardParam.CRASHED.value[0]] = is_crashed
self._reward_params_[
const.RewardParam.OFFTRACK.value[0]] = is_off_track
done = is_crashed or is_immobilized or is_off_track or is_lap_complete
episode_status = EpisodeStatus.get_episode_status(
is_crashed=is_crashed,
is_immobilized=is_immobilized,
is_off_track=is_off_track,
is_lap_complete=is_lap_complete)
try:
reward = float(self._reward_(copy.deepcopy(self._reward_params_)))
except Exception as ex:
raise RewardFunctionError(
'Reward function exception {}'.format(ex))
if math.isnan(reward) or math.isinf(reward):
raise RewardFunctionError('{} returned as reward'.format(reward))
self._prev_waypoints_['prev_point_2'] = self._prev_waypoints_[
'prev_point']
self._prev_waypoints_['prev_point'] = model_point
self._data_dict_['prev_progress'] = current_progress
#Get the last of the step metrics
self._step_metrics_[StepMetrics.REWARD.value] = reward
self._step_metrics_[StepMetrics.DONE.value] = done
self._step_metrics_[StepMetrics.TIME.value] = time.time()
self._step_metrics_[
StepMetrics.EPISODE_STATUS.value] = episode_status.value
return reward, done, self._step_metrics_
def finish_episode(self):
if self._start_pos_behavior_['change_start']:
self._data_dict_['start_ndist'] = (
self._data_dict_['start_ndist'] +
const.ROUND_ROBIN_ADVANCE_DIST) % 1.0
if self._start_pos_behavior_['alternate_dir']:
self._reverse_dir_ = not self._reverse_dir_
def clear_data(self):
for key in self._prev_waypoints_:
self._prev_waypoints_[key] = Point(0, 0)
for key in self._data_dict_:
if key != 'start_ndist':
self._data_dict_[key] = 0.0