Advanced workshops/reInvent2019-400/customize/deepracer_racetrack_env_lidar_3cars.py [262:523]:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        rospy.wait_for_service('/gazebo/set_model_state')
        rospy.wait_for_service('/gazebo/spawn_urdf_model')
        self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        self.spawn_model_client = rospy.ServiceProxy('gazebo/spawn_urdf_model', SpawnModel)

        # Spawn the new car
        robot_description = rospy.get_param('robot_description_bot')

        car_initial_pose = Pose()
        car_initial_pose.position.x = 0
        car_initial_pose.position.y = 0
        car_initial_pose.position.z = 0
        self.car_name = "racecar_{}".format(car_id)
        self.car_section = car_id - 1    # car_id should be 1, 2 and 3
        self.is_inner = is_inner
        self.resp = self.spawn_model_client(self.car_name,
                                            robot_description,
                                            '/{}'.format(self.car_name),
                                            car_initial_pose, '')
        logger.info('Spawn status: {}'.format(self.resp.status_message))

        # Initialize member data and reset car
        self.car_speed = speed

        # Initializing the lane that the bot car runs
        self.lanes_list_np = lanes_list_np
        self.lanes_one_hot = np.zeros(len(lanes_list_np), dtype=bool)
        if is_inner:
            self.lanes_one_hot[0] = True
            self.shapely_lane = lanes_list_np[self.lanes_one_hot][0]
        else:
            self.lanes_one_hot[1] = True
            self.shapely_lane = lanes_list_np[self.lanes_one_hot][0]

        # Change the lanes after certain time
        self.change_lane_freq_sec = 30 if self.car_section==self.stat_sec else 0
        self.reverse_dir = False

        self.car_model_state = ModelState()
        self.reset_sim(self.sec_map[self.is_inner][self.car_section][0])

    def reset_sim(self, start_dist):
        rostime = rospy.get_rostime()
        self.car_initial_dist = start_dist
        self.car_initial_time = rostime.secs + 1.e-9*rostime.nsecs
        self.next_change_lane_time = self.car_initial_time + self.change_lane_freq_sec
        self.position_bot_car(start_dist)

    def update_bot_sim(self, sim_time):
        current_time = sim_time.clock.secs + 1.e-9*sim_time.clock.nsecs
        seconds_elapsed = current_time - self.car_initial_time
        traveled_dist = seconds_elapsed * self.car_speed
        if self.reverse_dir:
            current_dist = (self.car_initial_dist - traveled_dist) % self.shapely_lane.length
        else:
            current_dist = (self.car_initial_dist + traveled_dist) % self.shapely_lane.length

        # Check for lane change and section reset
        if self.car_section == self.stat_sec and current_time > self.next_change_lane_time:
            # Get the next lane index
            tmp_lane_np = np.zeros(len(self.lanes_list_np), dtype=bool)
            next_lane_idx = (int(np.argwhere(self.lanes_one_hot==True)) + 1) % len(self.lanes_list_np)
            tmp_lane_np[next_lane_idx] = True
            self.lanes_one_hot = tmp_lane_np
            self.is_inner = not self.is_inner

            # Assigns the next lane that the bot car as to travel
            self.shapely_lane = self.lanes_list_np[self.lanes_one_hot][0]
            self.reset_sim(self.sec_map[self.is_inner][self.car_section][0])
        elif self.car_section != self.stat_sec and current_dist >= self.sec_map[self.is_inner][self.car_section][1]:
            tmp_lane_np = np.zeros(len(self.lanes_list_np), dtype=bool)
            next_lane_idx = (int(np.argwhere(self.lanes_one_hot==True)) + 1) % len(self.lanes_list_np)
            tmp_lane_np[next_lane_idx] = True
            self.lanes_one_hot = tmp_lane_np
            self.is_inner = not self.is_inner
            
            self.shapely_lane = self.lanes_list_np[self.lanes_one_hot][0]
            self.reset_sim(self.sec_map[self.is_inner][self.car_section][0])
        else:
            # Position the car on the lane
            self.position_bot_car(current_dist)

    def position_bot_car(self, car_dist):
        # Set the position of the second car
        _, next_index = self.find_prev_next_lane_points(car_dist, self.reverse_dir)
        car_position = self.shapely_lane.interpolate(car_dist)
        car_yaw = math.atan2(self.shapely_lane.coords[next_index][1] - car_position.y,
                             self.shapely_lane.coords[next_index][0] - car_position.x)
        car_quaternion = Rotation.from_euler('zyx', [car_yaw, 0, 0]).as_quat()
        self.car_model_state.model_name = self.car_name
        self.car_model_state.pose.position.x = car_position.x
        self.car_model_state.pose.position.y = car_position.y
        self.car_model_state.pose.position.z = 0
        self.car_model_state.pose.orientation.x = car_quaternion[0]
        self.car_model_state.pose.orientation.y = car_quaternion[1]
        self.car_model_state.pose.orientation.z = car_quaternion[2]
        self.car_model_state.pose.orientation.w = car_quaternion[3]
        self.car_model_state.twist.linear.x = 0
        self.car_model_state.twist.linear.y = 0
        self.car_model_state.twist.linear.z = 0
        self.car_model_state.twist.angular.x = 0
        self.car_model_state.twist.angular.y = 0
        self.car_model_state.twist.angular.z = 0
        self.set_model_state(self.car_model_state)

    def get_lane_dists(self, lane):
        return [lane.project(Point(p)) for p in lane.coords[:-1]] + [1.0]
    
    def find_prev_next_lane_points(self, ndist, reverse_dir):
        lane_dist = self.get_lane_dists(self.shapely_lane)
        if reverse_dir:
            next_index = bisect.bisect_left(lane_dist, ndist) - 1
            prev_index = next_index + 1
            if next_index == -1: next_index = len(lane_dist) - 1
        else:
            next_index = bisect.bisect_right(lane_dist, ndist)
            prev_index = next_index - 1
            if next_index == len(lane_dist): next_index = 0
        return prev_index, next_index
    
### Gym Env ###
class DeepRacerRacetrackEnv(gym.Env):

    def __init__(self):

        # Create the observation space
#         self.observation_space = spaces.Box(low=0, high=255,
#                                             shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 3),
#                                             dtype=np.uint8)
#         self.observation_space = spaces.Dict({
# #                 'left_camera': spaces.Box(low=0, high=255,
# #                                         shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 3),
# #                                         dtype=np.uint8),
#                 'stereo': spaces.Box(low=0, high=255,
#                                         shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 2),
#                                         dtype=np.uint8)
#         })
    
        self.observation_space = spaces.Dict({
#                 'left_camera': spaces.Box(low=0, high=255,
#                                         shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 3),
#                                         dtype=np.uint8),
                'STEREO_CAMERAS': spaces.Box(low=0, high=255,
                                        shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 2),
                                        dtype=np.uint8),
                'LIDAR': spaces.Box(low=0.15, high=1.0, shape=(64,))
        })
    
        # Create the action space
        self.action_space = spaces.Box(low=np.array([-1, 0]), high=np.array([+1, +1]), dtype=np.float32)

        if node_type == SIMULATION_WORKER:

            # ROS initialization
            rospy.init_node('rl_coach', anonymous=True)

            # wait for required services
            rospy.wait_for_service('/deepracer_simulation_environment/get_waypoints')
            rospy.wait_for_service('/deepracer_simulation_environment/reset_car')
            rospy.wait_for_service('/gazebo/get_model_state')
            rospy.wait_for_service('/gazebo/get_link_state')
            rospy.wait_for_service('/gazebo/clear_joint_forces')
            rospy.wait_for_service('/gazebo/spawn_urdf_model')

            self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
            self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
            self.clear_forces_client = rospy.ServiceProxy('/gazebo/clear_joint_forces', JointRequest)
            self.reset_car_client = rospy.ServiceProxy('/deepracer_simulation_environment/reset_car', ResetCarSrv)
            get_waypoints_client = rospy.ServiceProxy('/deepracer_simulation_environment/get_waypoints', GetWaypointSrv)

            # Create the publishers for sending speed and steering info to the car
            self.velocity_pub_dict = OrderedDict()
            self.steering_pub_dict = OrderedDict()

            for topic in VELOCITY_TOPICS:
                self.velocity_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)

            for topic in STEERING_TOPICS:
                self.steering_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)

            # Read in parameters
            self.world_name = rospy.get_param('WORLD_NAME')
            self.job_type = rospy.get_param('JOB_TYPE')
            self.aws_region = rospy.get_param('AWS_REGION')
            self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
            self.metrics_s3_object_key = rospy.get_param('METRICS_S3_OBJECT_KEY')
            self.metrics = []
            self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
                                      rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
                                      ':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')

            # Setup SIM_TRACE_LOG data upload to s3
            # self.setup_simtrace_data_upload_to_s3()

            if self.job_type == TRAINING_JOB:
                from custom_files.customer_reward_function import reward_function
                self.reward_function = reward_function
                self.target_number_of_episodes = rospy.get_param('NUMBER_OF_EPISODES')
                self.target_reward_score = rospy.get_param('TARGET_REWARD_SCORE')
            else:
                from markov.defaults import reward_function
                self.reward_function = reward_function
                self.number_of_trials = 0
                self.target_number_of_trials = rospy.get_param('NUMBER_OF_TRIALS')

            # Request the waypoints
            waypoints = None
            try:
                resp = get_waypoints_client()
                waypoints = np.array(resp.waypoints).reshape(resp.row, resp.col)
            except Exception as ex:
                utils.json_format_logger("Unable to retrieve waypoints: {}".format(ex),
                             **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION,
                                                             utils.SIMAPP_EVENT_ERROR_CODE_500))

            is_loop = np.all(waypoints[0,:] == waypoints[-1,:])
            if 'inch' in self.world_name:
                coef = 0.5
            else:
                coef = 0.6
            if is_loop:
                self.center_line = LinearRing(waypoints[:,0:2])
                self.inner_border = LinearRing(waypoints[:,2:4])
                self.outer_border = LinearRing(waypoints[:,4:6])
                self.left_lane = LinearRing(waypoints[:, 2:4]*coef + waypoints[:, 0:2]*(1-coef))
                self.right_lane = LinearRing(waypoints[:, 4:6]*coef + waypoints[:, 0:2]*(1-coef))
                self.road_poly = Polygon(self.outer_border, [self.inner_border])
            else:
                self.center_line = LineString(waypoints[:,0:2])
                self.inner_border = LineString(waypoints[:,2:4])
                self.outer_border = LineString(waypoints[:,4:6])
                self.left_lane = LinearRing(waypoints[:, 2:4]*coef + waypoints[:, 0:2]*(1-coef))
                self.right_lane = LinearRing(waypoints[:, 4:6]*coef + waypoints[:, 0:2]*(1-coef))
                self.road_poly = Polygon(np.vstack((self.outer_border, np.flipud(self.inner_border))))
            self.center_dists = [self.center_line.project(Point(p), normalized=True) for p in self.center_line.coords[:-1]] + [1.0]
            self.track_length = self.center_line.length
            
            self.safe_angle = 30
            
            # Queue used to maintain image consumption synchronicity
            self.image_queue_left_camera = queue.Queue(IMG_QUEUE_BUF_SIZE)
            rospy.Subscriber('/racecar/camera/zed/rgb/image_rect_color', sensor_image, self.callback_image_left_camera)
            self.image_queue_right_camera = queue.Queue(IMG_QUEUE_BUF_SIZE)
            rospy.Subscriber('/racecar/camera/zed_right/rgb/image_rect_color_right', sensor_image, self.callback_image_right_camera)
            print(self)

            # Initialize state data
            self.episodes = 0
            self.reverse_dir = False
#             if self.reverse_dir:
#                 self.start_ndist_map = [0.28, 0.5, 0.97]
#             else:
#                 self.start_ndist_map = [0.2, 0.42, 0.78]    # 36inch
# #                 self.start_ndist_map = [0.19, 0.67, 0.87]    # base

            if 'base' in self.world_name:
                self.start_ndist_map = [0.19, 0.67, 0.87]
            elif 'inch' in self.world_name:
                self.start_ndist_map = [0.23, 0.45, 0.93]
            elif 'mirror' in self.world_name:
                self.start_ndist_map = [0.12, 0.34, 0.83]
            else:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -



Advanced workshops/reInvent2019-400/src/markov/environments/deepracer_racetrack_env.py [143:402]:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        rospy.wait_for_service('/gazebo/set_model_state')
        rospy.wait_for_service('/gazebo/spawn_urdf_model')
        self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        self.spawn_model_client = rospy.ServiceProxy('gazebo/spawn_urdf_model', SpawnModel)

        # Spawn the new car
        robot_description = rospy.get_param('robot_description_bot')

        car_initial_pose = Pose()
        car_initial_pose.position.x = 0
        car_initial_pose.position.y = 0
        car_initial_pose.position.z = 0
        self.car_name = "racecar_{}".format(car_id)
        self.car_section = car_id - 1    # car_id should be 1, 2 and 3
        self.is_inner = is_inner
        self.resp = self.spawn_model_client(self.car_name,
                                            robot_description,
                                            '/{}'.format(self.car_name),
                                            car_initial_pose, '')
        logger.info('Spawn status: {}'.format(self.resp.status_message))

        # Initialize member data and reset car
        self.car_speed = speed

        # Initializing the lane that the bot car runs
        self.lanes_list_np = lanes_list_np
        self.lanes_one_hot = np.zeros(len(lanes_list_np), dtype=bool)
        if is_inner:
            self.lanes_one_hot[0] = True
            self.shapely_lane = lanes_list_np[self.lanes_one_hot][0]
        else:
            self.lanes_one_hot[1] = True
            self.shapely_lane = lanes_list_np[self.lanes_one_hot][0]

        # Change the lanes after certain time
        self.change_lane_freq_sec = 30 if self.car_section==self.stat_sec else 0
        self.reverse_dir = False

        self.car_model_state = ModelState()
        self.reset_sim(self.sec_map[self.is_inner][self.car_section][0])

    def reset_sim(self, start_dist):
        rostime = rospy.get_rostime()
        self.car_initial_dist = start_dist
        self.car_initial_time = rostime.secs + 1.e-9*rostime.nsecs
        self.next_change_lane_time = self.car_initial_time + self.change_lane_freq_sec
        self.position_bot_car(start_dist)

    def update_bot_sim(self, sim_time):
        current_time = sim_time.clock.secs + 1.e-9*sim_time.clock.nsecs
        seconds_elapsed = current_time - self.car_initial_time
        traveled_dist = seconds_elapsed * self.car_speed
        if self.reverse_dir:
            current_dist = (self.car_initial_dist - traveled_dist) % self.shapely_lane.length
        else:
            current_dist = (self.car_initial_dist + traveled_dist) % self.shapely_lane.length

        
        # Check for lane change and section reset
        if self.car_section == self.stat_sec and current_time > self.next_change_lane_time:
            # Get the next lane index
            tmp_lane_np = np.zeros(len(self.lanes_list_np), dtype=bool)
            next_lane_idx = (int(np.argwhere(self.lanes_one_hot==True)) + 1) % len(self.lanes_list_np)
            tmp_lane_np[next_lane_idx] = True
            self.lanes_one_hot = tmp_lane_np
            self.is_inner = not self.is_inner

            # Assigns the next lane that the bot car as to travel
            self.shapely_lane = self.lanes_list_np[self.lanes_one_hot][0]
            self.reset_sim(self.sec_map[self.is_inner][self.car_section][0])
        elif self.car_section != self.stat_sec and current_dist >= self.sec_map[self.is_inner][self.car_section][1]:
            tmp_lane_np = np.zeros(len(self.lanes_list_np), dtype=bool)
            next_lane_idx = (int(np.argwhere(self.lanes_one_hot==True)) + 1) % len(self.lanes_list_np)
            tmp_lane_np[next_lane_idx] = True
            self.lanes_one_hot = tmp_lane_np
            self.is_inner = not self.is_inner
            
            self.shapely_lane = self.lanes_list_np[self.lanes_one_hot][0]
            self.reset_sim(self.sec_map[self.is_inner][self.car_section][0])
        else:
            # Position the car on the lane
            self.position_bot_car(current_dist)

    def position_bot_car(self, car_dist):
        # Set the position of the second car
        _, next_index = self.find_prev_next_lane_points(car_dist, self.reverse_dir)
        car_position = self.shapely_lane.interpolate(car_dist)
        car_yaw = math.atan2(self.shapely_lane.coords[next_index][1] - car_position.y,
                             self.shapely_lane.coords[next_index][0] - car_position.x)
        car_quaternion = Rotation.from_euler('zyx', [car_yaw, 0, 0]).as_quat()
        self.car_model_state.model_name = self.car_name
        self.car_model_state.pose.position.x = car_position.x
        self.car_model_state.pose.position.y = car_position.y
        self.car_model_state.pose.position.z = 0
        self.car_model_state.pose.orientation.x = car_quaternion[0]
        self.car_model_state.pose.orientation.y = car_quaternion[1]
        self.car_model_state.pose.orientation.z = car_quaternion[2]
        self.car_model_state.pose.orientation.w = car_quaternion[3]
        self.car_model_state.twist.linear.x = 0
        self.car_model_state.twist.linear.y = 0
        self.car_model_state.twist.linear.z = 0
        self.car_model_state.twist.angular.x = 0
        self.car_model_state.twist.angular.y = 0
        self.car_model_state.twist.angular.z = 0
        self.set_model_state(self.car_model_state)

    def get_lane_dists(self, lane):
        return [lane.project(Point(p)) for p in lane.coords[:-1]] + [1.0]
    
    def find_prev_next_lane_points(self, ndist, reverse_dir):
        lane_dist = self.get_lane_dists(self.shapely_lane)
        if reverse_dir:
            next_index = bisect.bisect_left(lane_dist, ndist) - 1
            prev_index = next_index + 1
            if next_index == -1: next_index = len(lane_dist) - 1
        else:
            next_index = bisect.bisect_right(lane_dist, ndist)
            prev_index = next_index - 1
            if next_index == len(lane_dist): next_index = 0
        return prev_index, next_index
    
### Gym Env ###
class DeepRacerRacetrackEnv(gym.Env):

    def __init__(self):

        # Create the observation space
#         self.observation_space = spaces.Box(low=0, high=255,
#                                             shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 3),
#                                             dtype=np.uint8)
#         self.observation_space = spaces.Dict({
# #                 'left_camera': spaces.Box(low=0, high=255,
# #                                         shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 3),
# #                                         dtype=np.uint8),
#                 'stereo': spaces.Box(low=0, high=255,
#                                         shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 2),
#                                         dtype=np.uint8)
#         })
    
        self.observation_space = spaces.Dict({
                'STEREO_CAMERAS': spaces.Box(low=0, high=255,
                                        shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 2),
                                        dtype=np.uint8),
                'LIDAR': spaces.Box(low=0.15, high=1.0, shape=(64,))
        })
    
        # Create the action space
        self.action_space = spaces.Box(low=np.array([-1, 0]), high=np.array([+1, +1]), dtype=np.float32)

        if node_type == SIMULATION_WORKER:

            # ROS initialization
            rospy.init_node('rl_coach', anonymous=True)

            # wait for required services
            rospy.wait_for_service('/deepracer_simulation_environment/get_waypoints')
            rospy.wait_for_service('/deepracer_simulation_environment/reset_car')
            rospy.wait_for_service('/gazebo/get_model_state')
            rospy.wait_for_service('/gazebo/get_link_state')
            rospy.wait_for_service('/gazebo/clear_joint_forces')
            rospy.wait_for_service('/gazebo/spawn_urdf_model')

            self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
            self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
            self.clear_forces_client = rospy.ServiceProxy('/gazebo/clear_joint_forces', JointRequest)
            self.reset_car_client = rospy.ServiceProxy('/deepracer_simulation_environment/reset_car', ResetCarSrv)
            get_waypoints_client = rospy.ServiceProxy('/deepracer_simulation_environment/get_waypoints', GetWaypointSrv)

            # Create the publishers for sending speed and steering info to the car
            self.velocity_pub_dict = OrderedDict()
            self.steering_pub_dict = OrderedDict()

            for topic in VELOCITY_TOPICS:
                self.velocity_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)

            for topic in STEERING_TOPICS:
                self.steering_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)

            # Read in parameters
            self.world_name = rospy.get_param('WORLD_NAME')
            self.job_type = rospy.get_param('JOB_TYPE')
            self.aws_region = rospy.get_param('AWS_REGION')
            self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
            self.metrics_s3_object_key = rospy.get_param('METRICS_S3_OBJECT_KEY')
            self.metrics = []
            self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
                                      rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
                                      ':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')

            # Setup SIM_TRACE_LOG data upload to s3
            # self.setup_simtrace_data_upload_to_s3()

            if self.job_type == TRAINING_JOB:
                from custom_files.customer_reward_function import reward_function
                self.reward_function = reward_function
                self.target_number_of_episodes = rospy.get_param('NUMBER_OF_EPISODES')
                self.target_reward_score = rospy.get_param('TARGET_REWARD_SCORE')
            else:
                from markov.defaults import reward_function
                self.reward_function = reward_function
                self.number_of_trials = 0
                self.target_number_of_trials = rospy.get_param('NUMBER_OF_TRIALS')

            # Request the waypoints
            waypoints = None
            try:
                resp = get_waypoints_client()
                waypoints = np.array(resp.waypoints).reshape(resp.row, resp.col)
            except Exception as ex:
                utils.json_format_logger("Unable to retrieve waypoints: {}".format(ex),
                             **utils.build_system_error_dict(utils.SIMAPP_ENVIRONMENT_EXCEPTION,
                                                             utils.SIMAPP_EVENT_ERROR_CODE_500))

            is_loop = np.all(waypoints[0,:] == waypoints[-1,:])
            if 'inch' in self.world_name:
                coef = 0.5
            else:
                coef = 0.6
            if is_loop:
                self.center_line = LinearRing(waypoints[:,0:2])
                self.inner_border = LinearRing(waypoints[:,2:4])
                self.outer_border = LinearRing(waypoints[:,4:6])
                self.left_lane = LinearRing(waypoints[:, 2:4]*coef + waypoints[:, 0:2]*(1-coef))
                self.right_lane = LinearRing(waypoints[:, 4:6]*coef + waypoints[:, 0:2]*(1-coef))
                self.road_poly = Polygon(self.outer_border, [self.inner_border])
            else:
                self.center_line = LineString(waypoints[:,0:2])
                self.inner_border = LineString(waypoints[:,2:4])
                self.outer_border = LineString(waypoints[:,4:6])
                self.left_lane = LinearRing(waypoints[:, 2:4]*coef + waypoints[:, 0:2]*(1-coef))
                self.right_lane = LinearRing(waypoints[:, 4:6]*coef + waypoints[:, 0:2]*(1-coef))
                self.road_poly = Polygon(np.vstack((self.outer_border, np.flipud(self.inner_border))))
            self.center_dists = [self.center_line.project(Point(p), normalized=True) for p in self.center_line.coords[:-1]] + [1.0]
            self.track_length = self.center_line.length
            
            self.safe_angle = 30
            
            # Queue used to maintain image consumption synchronicity
            self.image_queue_left_camera = queue.Queue(IMG_QUEUE_BUF_SIZE)
            rospy.Subscriber('/racecar/camera/zed/rgb/image_rect_color', sensor_image, self.callback_image_left_camera)
            self.image_queue_right_camera = queue.Queue(IMG_QUEUE_BUF_SIZE)
            rospy.Subscriber('/racecar/camera/zed_right/rgb/image_rect_color_right', sensor_image, self.callback_image_right_camera)
            print(self)

            # Initialize state data
            self.episodes = 0
            self.reverse_dir = False
#             if self.reverse_dir:
#                 self.start_ndist_map = [0.28, 0.5, 0.97]
#             else:
#                 self.start_ndist_map = [0.2, 0.42, 0.78]    # 36inch
# #                 self.start_ndist_map = [0.19, 0.67, 0.87]    # base

            if 'base' in self.world_name:
                self.start_ndist_map = [0.19, 0.67, 0.87]
            elif 'inch' in self.world_name:
                self.start_ndist_map = [0.23, 0.45, 0.93]
            elif 'mirror' in self.world_name:
                self.start_ndist_map = [0.12, 0.34, 0.83]
            else:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -



