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 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_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 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 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): '''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 __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, 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)