Advanced workshops/AI Driving Olympics 2019/challenge_train_DQN/src/markov/environments/deepracer_racetrack_env.py [236:559]: - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - def racecar_reset(self): try: for joint in EFFORT_JOINTS: self.clear_forces_client(joint) prev_index, next_index = self.find_prev_next_waypoints(self.start_ndist) self.reset_car_client(self.start_ndist, next_index) # First clear the queue so that we set the state to the start image _ = self.image_queue.get(block=True, timeout=None) self.set_next_state() except Exception as ex: utils.json_format_logger("Unable to reset the car: {}".format(ex), **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_500)) def set_allow_servo_step_signals(self, allow_servo_step_signals): self.allow_servo_step_signals = allow_servo_step_signals def step(self, action): if node_type == SAGEMAKER_TRAINING_WORKER: return self.observation_space.sample(), 0, False, {} # Initialize next state, reward, done flag self.next_state = None self.reward = None self.done = False # Send this action to Gazebo and increment the step count self.steering_angle = float(action[0]) self.speed = float(action[1]) if self.allow_servo_step_signals: self.send_action(self.steering_angle, self.speed) self.steps += 1 # Compute the next state and reward self.infer_reward_state(self.steering_angle, self.speed) return self.next_state, self.reward, self.done, {} def callback_image(self, data): try: self.image_queue.put_nowait(data) except queue.Full: pass except Exception as ex: utils.json_format_logger("Error retrieving frame from gazebo: {}".format(ex), **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_500)) def send_action(self, steering_angle, speed): # Simple v/r to computes the desired rpm wheel_rpm = speed/WHEEL_RADIUS for _, pub in self.velocity_pub_dict.items(): pub.publish(wheel_rpm) for _, pub in self.steering_pub_dict.items(): pub.publish(steering_angle) def infer_reward_state(self, steering_angle, speed): try: self.set_next_state() except Exception as ex: utils.json_format_logger("Unable to retrieve image from queue: {}".format(ex), **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_500)) # Read model state from Gazebo model_state = self.get_model_state('racecar', '') model_orientation = Rotation.from_quat([ model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w]) model_location = np.array([ model_state.pose.position.x, model_state.pose.position.y, model_state.pose.position.z]) + \ model_orientation.apply(RELATIVE_POSITION_OF_FRONT_OF_CAR) model_point = Point(model_location[0], model_location[1]) model_heading = model_orientation.as_euler('zyx')[0] # Read the wheel locations from Gazebo left_rear_wheel_state = self.get_link_state('racecar::left_rear_wheel', '') left_front_wheel_state = self.get_link_state('racecar::left_front_wheel', '') right_rear_wheel_state = self.get_link_state('racecar::right_rear_wheel', '') right_front_wheel_state = self.get_link_state('racecar::right_front_wheel', '') wheel_points = [ Point(left_rear_wheel_state.link_state.pose.position.x, left_rear_wheel_state.link_state.pose.position.y), Point(left_front_wheel_state.link_state.pose.position.x, left_front_wheel_state.link_state.pose.position.y), Point(right_rear_wheel_state.link_state.pose.position.x, right_rear_wheel_state.link_state.pose.position.y), Point(right_front_wheel_state.link_state.pose.position.x, right_front_wheel_state.link_state.pose.position.y) ] # Project the current location onto the center line and find nearest points current_ndist = self.center_line.project(model_point, normalized=True) prev_index, next_index = self.find_prev_next_waypoints(current_ndist) distance_from_prev = model_point.distance(Point(self.center_line.coords[prev_index])) distance_from_next = model_point.distance(Point(self.center_line.coords[next_index])) closest_waypoint_index = (prev_index, next_index)[distance_from_next < distance_from_prev] # Compute distance from center and road width nearest_point_center = self.center_line.interpolate(current_ndist, normalized=True) nearest_point_inner = self.inner_border.interpolate(self.inner_border.project(nearest_point_center)) nearest_point_outer = self.outer_border.interpolate(self.outer_border.project(nearest_point_center)) distance_from_center = nearest_point_center.distance(model_point) distance_from_inner = nearest_point_inner.distance(model_point) distance_from_outer = nearest_point_outer.distance(model_point) track_width = nearest_point_inner.distance(nearest_point_outer) is_left_of_center = (distance_from_outer < distance_from_inner) if self.reverse_dir \ else (distance_from_inner < distance_from_outer) # Convert current progress to be [0,100] starting at the initial waypoint if self.reverse_dir: current_progress = self.start_ndist - current_ndist else: current_progress = current_ndist - self.start_ndist if current_progress < 0.0: current_progress = current_progress + 1.0 current_progress = 100 * current_progress if current_progress < self.prev_progress: # Either: (1) we wrapped around and have finished the track, delta1 = current_progress + 100 - self.prev_progress # or (2) for some reason the car went backwards (this should be rare) delta2 = self.prev_progress - current_progress current_progress = (self.prev_progress, 100)[delta1 < delta2] # Car is off track if all wheels are outside the borders wheel_on_track = [self.road_poly.contains(p) for p in wheel_points] all_wheels_on_track = all(wheel_on_track) any_wheels_on_track = any(wheel_on_track) # Compute the reward if any_wheels_on_track: done = False params = { 'all_wheels_on_track': all_wheels_on_track, 'x': model_point.x, 'y': model_point.y, 'heading': model_heading * 180.0 / math.pi, 'distance_from_center': distance_from_center, 'progress': current_progress, 'steps': self.steps, 'speed': speed, 'steering_angle': steering_angle * 180.0 / math.pi, 'track_width': track_width, 'waypoints': list(self.center_line.coords), 'closest_waypoints': [prev_index, next_index], 'is_left_of_center': is_left_of_center, 'is_reversed': self.reverse_dir } try: reward = float(self.reward_function(params)) except Exception as e: utils.json_format_logger("Exception {} in customer reward function. Job failed!".format(e), **utils.build_user_error_dict(utils.SIMAPP_SIMULATION_WORKER_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_400)) traceback.print_exc() sys.exit(1) else: done = True reward = CRASHED # Reset if the car position hasn't changed in the last 2 steps prev_pnt_dist = min(model_point.distance(self.prev_point), model_point.distance(self.prev_point_2)) if prev_pnt_dist <= 0.0001 and self.steps % NUM_STEPS_TO_CHECK_STUCK == 0: done = True reward = CRASHED # stuck # Simulation jobs are done when progress reaches 100 if current_progress >= 100: done = True # Keep data from the previous step around self.prev_point_2 = self.prev_point self.prev_point = model_point self.prev_progress = current_progress # Set the reward and done flag self.reward = reward self.reward_in_episode += reward self.done = done # Trace logs to help us debug and visualize the training runs # btown TODO: This should be written to S3, not to CWL. logger.info('SIM_TRACE_LOG:%d,%d,%.4f,%.4f,%.4f,%.2f,%.2f,%d,%.4f,%s,%s,%.4f,%d,%.2f,%s\n' % ( self.episodes, self.steps, model_location[0], model_location[1], model_heading, self.steering_angle, self.speed, self.action_taken, self.reward, self.done, all_wheels_on_track, current_progress, closest_waypoint_index, self.track_length, time.time())) # Terminate this episode when ready if done and node_type == SIMULATION_WORKER: self.finish_episode(current_progress) def find_prev_next_waypoints(self, ndist): if self.reverse_dir: next_index = bisect.bisect_left(self.center_dists, ndist) - 1 prev_index = next_index + 1 if next_index == -1: next_index = len(self.center_dists) - 1 else: next_index = bisect.bisect_right(self.center_dists, ndist) prev_index = next_index - 1 if next_index == len(self.center_dists): next_index = 0 return prev_index, next_index def stop_car(self): self.steering_angle = 0 self.speed = 0 self.action_taken = 0 self.send_action(0, 0) self.racecar_reset() def finish_episode(self, progress): # Increment episode count, update start position and direction self.episodes += 1 if self.change_start: self.start_ndist = (self.start_ndist + ROUND_ROBIN_ADVANCE_DIST) % 1.0 if self.alternate_dir: self.reverse_dir = not self.reverse_dir # Reset the car self.stop_car() # Update metrics based on job type if self.job_type == TRAINING_JOB: self.send_reward_to_cloudwatch(self.reward_in_episode) self.update_training_metrics() self.write_metrics_to_s3() if self.is_training_done(): self.cancel_simulation_job() elif self.job_type == EVALUATION_JOB: self.number_of_trials += 1 self.update_eval_metrics(progress) self.write_metrics_to_s3() def update_eval_metrics(self, progress): eval_metric = {} eval_metric['completion_percentage'] = int(progress) eval_metric['metric_time'] = int(round(time.time() * 1000)) eval_metric['start_time'] = int(round(self.simulation_start_time * 1000)) eval_metric['elapsed_time_in_milliseconds'] = int(round((time.time() - self.simulation_start_time) * 1000)) eval_metric['trial'] = int(self.number_of_trials) self.metrics.append(eval_metric) def update_training_metrics(self): training_metric = {} training_metric['reward_score'] = int(round(self.reward_in_episode)) training_metric['metric_time'] = int(round(time.time() * 1000)) training_metric['start_time'] = int(round(self.simulation_start_time * 1000)) training_metric['elapsed_time_in_milliseconds'] = int(round((time.time() - self.simulation_start_time) * 1000)) training_metric['episode'] = int(self.episodes) self.metrics.append(training_metric) def write_metrics_to_s3(self): session = boto3.session.Session() s3_client = session.client('s3', region_name=self.aws_region) metrics_body = json.dumps({'metrics': self.metrics}) s3_client.put_object( Bucket=self.metrics_s3_bucket, Key=self.metrics_s3_object_key, Body=bytes(metrics_body, encoding='utf-8') ) def is_training_done(self): if ((self.target_number_of_episodes > 0) and (self.target_number_of_episodes == self.episodes)) or \ ((isinstance(self.target_reward_score, (int, float))) and (self.target_reward_score <= self.reward_in_episode)): self.is_simulation_done = True return self.is_simulation_done def cancel_simulation_job(self): session = boto3.session.Session() robomaker_client = session.client('robomaker', region_name=self.aws_region) robomaker_client.cancel_simulation_job( job=self.simulation_job_arn ) def send_reward_to_cloudwatch(self, reward): session = boto3.session.Session() cloudwatch_client = session.client('cloudwatch', region_name=self.aws_region) cloudwatch_client.put_metric_data( MetricData=[ { 'MetricName': self.metric_name, 'Dimensions': [ { 'Name': 'TRAINING_JOB_ARN', 'Value': self.training_job_arn }, ], 'Unit': 'None', 'Value': reward }, ], Namespace=self.metric_namespace ) class DeepRacerRacetrackCustomActionSpaceEnv(DeepRacerRacetrackEnv): def __init__(self): DeepRacerRacetrackEnv.__init__(self) try: # Try loading the custom model metadata (may or may not be present) with open('./custom_files/model_metadata.json', 'r') as f: model_metadata = json.load(f) self.json_actions = model_metadata['action_space'] logger.info("Loaded action space from file: {}".format(self.json_actions)) except Exception as ex: # Failed to load, fall back on the default action space from markov.defaults import model_metadata self.json_actions = model_metadata['action_space'] logger.info("Exception {} on loading custom action space, using default: {}".format(ex, self.json_actions)) self.action_space = spaces.Discrete(len(self.json_actions)) def step(self, action): self.steering_angle = float(self.json_actions[action]['steering_angle']) * math.pi / 180.0 self.speed = float(self.json_actions[action]['speed']) self.action_taken = action return super().step([self.steering_angle, self.speed]) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Advanced workshops/AI Driving Olympics 2019/challenge_train_w_PPO/src/markov/environments/deepracer_racetrack_env_cv2.py [256:579]: - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - def racecar_reset(self): try: for joint in EFFORT_JOINTS: self.clear_forces_client(joint) prev_index, next_index = self.find_prev_next_waypoints(self.start_ndist) self.reset_car_client(self.start_ndist, next_index) # First clear the queue so that we set the state to the start image _ = self.image_queue.get(block=True, timeout=None) self.set_next_state() except Exception as ex: utils.json_format_logger("Unable to reset the car: {}".format(ex), **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_500)) def set_allow_servo_step_signals(self, allow_servo_step_signals): self.allow_servo_step_signals = allow_servo_step_signals def step(self, action): if node_type == SAGEMAKER_TRAINING_WORKER: return self.observation_space.sample(), 0, False, {} # Initialize next state, reward, done flag self.next_state = None self.reward = None self.done = False # Send this action to Gazebo and increment the step count self.steering_angle = float(action[0]) self.speed = float(action[1]) if self.allow_servo_step_signals: self.send_action(self.steering_angle, self.speed) self.steps += 1 # Compute the next state and reward self.infer_reward_state(self.steering_angle, self.speed) return self.next_state, self.reward, self.done, {} def callback_image(self, data): try: self.image_queue.put_nowait(data) except queue.Full: pass except Exception as ex: utils.json_format_logger("Error retrieving frame from gazebo: {}".format(ex), **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_500)) def send_action(self, steering_angle, speed): # Simple v/r to computes the desired rpm wheel_rpm = speed/WHEEL_RADIUS for _, pub in self.velocity_pub_dict.items(): pub.publish(wheel_rpm) for _, pub in self.steering_pub_dict.items(): pub.publish(steering_angle) def infer_reward_state(self, steering_angle, speed): try: self.set_next_state() except Exception as ex: utils.json_format_logger("Unable to retrieve image from queue: {}".format(ex), **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_500)) # Read model state from Gazebo model_state = self.get_model_state('racecar', '') model_orientation = Rotation.from_quat([ model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w]) model_location = np.array([ model_state.pose.position.x, model_state.pose.position.y, model_state.pose.position.z]) + \ model_orientation.apply(RELATIVE_POSITION_OF_FRONT_OF_CAR) model_point = Point(model_location[0], model_location[1]) model_heading = model_orientation.as_euler('zyx')[0] # Read the wheel locations from Gazebo left_rear_wheel_state = self.get_link_state('racecar::left_rear_wheel', '') left_front_wheel_state = self.get_link_state('racecar::left_front_wheel', '') right_rear_wheel_state = self.get_link_state('racecar::right_rear_wheel', '') right_front_wheel_state = self.get_link_state('racecar::right_front_wheel', '') wheel_points = [ Point(left_rear_wheel_state.link_state.pose.position.x, left_rear_wheel_state.link_state.pose.position.y), Point(left_front_wheel_state.link_state.pose.position.x, left_front_wheel_state.link_state.pose.position.y), Point(right_rear_wheel_state.link_state.pose.position.x, right_rear_wheel_state.link_state.pose.position.y), Point(right_front_wheel_state.link_state.pose.position.x, right_front_wheel_state.link_state.pose.position.y) ] # Project the current location onto the center line and find nearest points current_ndist = self.center_line.project(model_point, normalized=True) prev_index, next_index = self.find_prev_next_waypoints(current_ndist) distance_from_prev = model_point.distance(Point(self.center_line.coords[prev_index])) distance_from_next = model_point.distance(Point(self.center_line.coords[next_index])) closest_waypoint_index = (prev_index, next_index)[distance_from_next < distance_from_prev] # Compute distance from center and road width nearest_point_center = self.center_line.interpolate(current_ndist, normalized=True) nearest_point_inner = self.inner_border.interpolate(self.inner_border.project(nearest_point_center)) nearest_point_outer = self.outer_border.interpolate(self.outer_border.project(nearest_point_center)) distance_from_center = nearest_point_center.distance(model_point) distance_from_inner = nearest_point_inner.distance(model_point) distance_from_outer = nearest_point_outer.distance(model_point) track_width = nearest_point_inner.distance(nearest_point_outer) is_left_of_center = (distance_from_outer < distance_from_inner) if self.reverse_dir \ else (distance_from_inner < distance_from_outer) # Convert current progress to be [0,100] starting at the initial waypoint if self.reverse_dir: current_progress = self.start_ndist - current_ndist else: current_progress = current_ndist - self.start_ndist if current_progress < 0.0: current_progress = current_progress + 1.0 current_progress = 100 * current_progress if current_progress < self.prev_progress: # Either: (1) we wrapped around and have finished the track, delta1 = current_progress + 100 - self.prev_progress # or (2) for some reason the car went backwards (this should be rare) delta2 = self.prev_progress - current_progress current_progress = (self.prev_progress, 100)[delta1 < delta2] # Car is off track if all wheels are outside the borders wheel_on_track = [self.road_poly.contains(p) for p in wheel_points] all_wheels_on_track = all(wheel_on_track) any_wheels_on_track = any(wheel_on_track) # Compute the reward if any_wheels_on_track: done = False params = { 'all_wheels_on_track': all_wheels_on_track, 'x': model_point.x, 'y': model_point.y, 'heading': model_heading * 180.0 / math.pi, 'distance_from_center': distance_from_center, 'progress': current_progress, 'steps': self.steps, 'speed': speed, 'steering_angle': steering_angle * 180.0 / math.pi, 'track_width': track_width, 'waypoints': list(self.center_line.coords), 'closest_waypoints': [prev_index, next_index], 'is_left_of_center': is_left_of_center, 'is_reversed': self.reverse_dir } try: reward = float(self.reward_function(params)) except Exception as e: utils.json_format_logger("Exception {} in customer reward function. Job failed!".format(e), **utils.build_user_error_dict(utils.SIMAPP_SIMULATION_WORKER_EXCEPTION, utils.SIMAPP_EVENT_ERROR_CODE_400)) traceback.print_exc() sys.exit(1) else: done = True reward = CRASHED # Reset if the car position hasn't changed in the last 2 steps prev_pnt_dist = min(model_point.distance(self.prev_point), model_point.distance(self.prev_point_2)) if prev_pnt_dist <= 0.0001 and self.steps % NUM_STEPS_TO_CHECK_STUCK == 0: done = True reward = CRASHED # stuck # Simulation jobs are done when progress reaches 100 if current_progress >= 100: done = True # Keep data from the previous step around self.prev_point_2 = self.prev_point self.prev_point = model_point self.prev_progress = current_progress # Set the reward and done flag self.reward = reward self.reward_in_episode += reward self.done = done # Trace logs to help us debug and visualize the training runs # btown TODO: This should be written to S3, not to CWL. logger.info('SIM_TRACE_LOG:%d,%d,%.4f,%.4f,%.4f,%.2f,%.2f,%d,%.4f,%s,%s,%.4f,%d,%.2f,%s\n' % ( self.episodes, self.steps, model_location[0], model_location[1], model_heading, self.steering_angle, self.speed, self.action_taken, self.reward, self.done, all_wheels_on_track, current_progress, closest_waypoint_index, self.track_length, time.time())) # Terminate this episode when ready if done and node_type == SIMULATION_WORKER: self.finish_episode(current_progress) def find_prev_next_waypoints(self, ndist): if self.reverse_dir: next_index = bisect.bisect_left(self.center_dists, ndist) - 1 prev_index = next_index + 1 if next_index == -1: next_index = len(self.center_dists) - 1 else: next_index = bisect.bisect_right(self.center_dists, ndist) prev_index = next_index - 1 if next_index == len(self.center_dists): next_index = 0 return prev_index, next_index def stop_car(self): self.steering_angle = 0 self.speed = 0 self.action_taken = 0 self.send_action(0, 0) self.racecar_reset() def finish_episode(self, progress): # Increment episode count, update start position and direction self.episodes += 1 if self.change_start: self.start_ndist = (self.start_ndist + ROUND_ROBIN_ADVANCE_DIST) % 1.0 if self.alternate_dir: self.reverse_dir = not self.reverse_dir # Reset the car self.stop_car() # Update metrics based on job type if self.job_type == TRAINING_JOB: self.send_reward_to_cloudwatch(self.reward_in_episode) self.update_training_metrics() self.write_metrics_to_s3() if self.is_training_done(): self.cancel_simulation_job() elif self.job_type == EVALUATION_JOB: self.number_of_trials += 1 self.update_eval_metrics(progress) self.write_metrics_to_s3() def update_eval_metrics(self, progress): eval_metric = {} eval_metric['completion_percentage'] = int(progress) eval_metric['metric_time'] = int(round(time.time() * 1000)) eval_metric['start_time'] = int(round(self.simulation_start_time * 1000)) eval_metric['elapsed_time_in_milliseconds'] = int(round((time.time() - self.simulation_start_time) * 1000)) eval_metric['trial'] = int(self.number_of_trials) self.metrics.append(eval_metric) def update_training_metrics(self): training_metric = {} training_metric['reward_score'] = int(round(self.reward_in_episode)) training_metric['metric_time'] = int(round(time.time() * 1000)) training_metric['start_time'] = int(round(self.simulation_start_time * 1000)) training_metric['elapsed_time_in_milliseconds'] = int(round((time.time() - self.simulation_start_time) * 1000)) training_metric['episode'] = int(self.episodes) self.metrics.append(training_metric) def write_metrics_to_s3(self): session = boto3.session.Session() s3_client = session.client('s3', region_name=self.aws_region) metrics_body = json.dumps({'metrics': self.metrics}) s3_client.put_object( Bucket=self.metrics_s3_bucket, Key=self.metrics_s3_object_key, Body=bytes(metrics_body, encoding='utf-8') ) def is_training_done(self): if ((self.target_number_of_episodes > 0) and (self.target_number_of_episodes == self.episodes)) or \ ((isinstance(self.target_reward_score, (int, float))) and (self.target_reward_score <= self.reward_in_episode)): self.is_simulation_done = True return self.is_simulation_done def cancel_simulation_job(self): session = boto3.session.Session() robomaker_client = session.client('robomaker', region_name=self.aws_region) robomaker_client.cancel_simulation_job( job=self.simulation_job_arn ) def send_reward_to_cloudwatch(self, reward): session = boto3.session.Session() cloudwatch_client = session.client('cloudwatch', region_name=self.aws_region) cloudwatch_client.put_metric_data( MetricData=[ { 'MetricName': self.metric_name, 'Dimensions': [ { 'Name': 'TRAINING_JOB_ARN', 'Value': self.training_job_arn }, ], 'Unit': 'None', 'Value': reward }, ], Namespace=self.metric_namespace ) class DeepRacerRacetrackCustomActionSpaceEnv(DeepRacerRacetrackEnv): def __init__(self): DeepRacerRacetrackEnv.__init__(self) try: # Try loading the custom model metadata (may or may not be present) with open('./custom_files/model_metadata.json', 'r') as f: model_metadata = json.load(f) self.json_actions = model_metadata['action_space'] logger.info("Loaded action space from file: {}".format(self.json_actions)) except Exception as ex: # Failed to load, fall back on the default action space from markov.defaults import model_metadata self.json_actions = model_metadata['action_space'] logger.info("Exception {} on loading custom action space, using default: {}".format(ex, self.json_actions)) self.action_space = spaces.Discrete(len(self.json_actions)) def step(self, action): self.steering_angle = float(self.json_actions[action]['steering_angle']) * math.pi / 180.0 self.speed = float(self.json_actions[action]['speed']) self.action_taken = action return super().step([self.steering_angle, self.speed]) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -