def ready(self): # print (self.planner) if self.planner == 'new': self._local_planner = LocalPlannerNew(self._player, 2.5, 9.0, 1.5) else: self._local_planner = LocalPlannerOld(self._player) self._local_planner.set_route(self._start_pose.location, self._target_pose.location) self.dist_start_to_goal = self._local_planner.distance_to_goal self.dist_current_to_goal = self._local_planner.distance_to_goal self._timeout = self._local_planner.calculate_timeout() # Initialize tests that do not build sensors here # self.collision_test = CollisionTest(self._player) self.route_comp_test = RouteCompletionTest( self._player, route=self._local_planner.get_route(), carla_map=self._map) self.run_red_test = RunningRedLightTest(self._player, carla_map=self._map) self.run_stop_test = RunningStopTest(self._player, world=self._world, carla_map=self._map) return super().ready()
def ready(self): # print (self.planner) if self.planner == 'new': self._local_planner = LocalPlannerNew(self._player, 2.5, 9.0, 1.5) else: self._local_planner = LocalPlannerOld(self._player) self._local_planner.set_route(self._start_pose.location, self._target_pose.location) self._timeout = self._local_planner.calculate_timeout() return super().ready()
def ready(self): # print (self.planner) if self.planner == "new": self._local_planner = LocalPlannerNew( self._player, resolution=2.5, threshold_before=25.0, threshold_after=1.5 ) else: self._local_planner = LocalPlannerOld(self._player) self._local_planner.set_route(self._start_pose.location, self._target_pose.location) self._timeout = self._local_planner.calculate_timeout() return super().ready()
class PointGoalSuite(BaseSuite): def __init__(self, success_dist=5.0, col_is_failure=False, viz_camera=False, planner='new', poses_txt='', **kwargs): super().__init__(**kwargs) self.success_dist = success_dist self.col_is_failure = col_is_failure self.planner = planner self.poses = from_file(poses_txt) self.command = RoadOption.LANEFOLLOW self.timestamp_active = 0 self._timeout = float('inf') self.viz_camera = viz_camera self._viz_queue = None self.collision_actors = [] self.route_completed = 0 self.red_count = 0 self.stop_count = 0 def init(self, target=1, **kwargs): self._target_pose = self._map.get_spawn_points()[target] super().init(**kwargs) def ready(self): # print (self.planner) if self.planner == 'new': self._local_planner = LocalPlannerNew(self._player, 2.5, 9.0, 1.5) else: self._local_planner = LocalPlannerOld(self._player) self._local_planner.set_route(self._start_pose.location, self._target_pose.location) self.dist_start_to_goal = self._local_planner.distance_to_goal self.dist_current_to_goal = self._local_planner.distance_to_goal self._timeout = self._local_planner.calculate_timeout() # Initialize tests that do not build sensors here # self.collision_test = CollisionTest(self._player) self.route_comp_test = RouteCompletionTest( self._player, route=self._local_planner.get_route(), carla_map=self._map) self.run_red_test = RunningRedLightTest(self._player, carla_map=self._map) self.run_stop_test = RunningStopTest(self._player, world=self._world, carla_map=self._map) return super().ready() def get_distance_start_to_goal(self): return self.dist_start_to_goal def tick(self): result = super().tick() self._local_planner.run_step() self.command = self._local_planner.checkpoint[1] self.node = self._local_planner.checkpoint[0].transform.location self._next = self._local_planner.target[0].transform.location # distance travelled already subtracted from start to goal self.dist_current_to_goal = self._local_planner.distance_to_goal self.collision_test.update() self.route_comp_test.update() self.run_red_test.update() self.run_stop_test.update() self.collision_actors = self.collision_test.collision_actor_type self.route_completed = self.route_comp_test.actual_value self.red_count = self.run_red_test.actual_value self.stop_count = self.run_stop_test.actual_value return result def get_observations(self): result = dict() result.update(super().get_observations()) result['command'] = int(self.command) result['node'] = np.array([self.node.x, self.node.y]) result['next'] = np.array([self._next.x, self._next.y]) result['dist_curr_to_goal'] = self.dist_current_to_goal return result @property def weathers(self): return self._weathers @property def pose_tasks(self): return self.poses def clean_up(self): super().clean_up() self.timestamp_active = 0 self._timeout = float('inf') self._local_planner = None # Clean-up cameras if self._viz_queue: with self._viz_queue.mutex: self._viz_queue.queue.clear() def is_failure(self): if self.timestamp_active >= self._timeout or self._tick >= 10000: return True elif self.col_is_failure and self.collided: return True return False def is_success(self): location = self._player.get_location() distance = location.distance(self._target_pose.location) return distance <= self.success_dist def apply_control(self, control): result = super().apply_control(control) # is_light_red = self._is_light_red(agent) # # if is_light_red: # self.timestamp_active -= 1 self.timestamp_active += 1 # Return diagnostics location = self._player.get_location() orientation = self._player.get_transform().get_forward_vector() velocity = self._player.get_velocity() speed = np.linalg.norm([velocity.x, velocity.y, velocity.z]) info = { 'x': location.x, 'y': location.y, 'z': location.z, 'ori_x': orientation.x, 'ori_y': orientation.y, 'speed': speed, 'collided': self.collided, 'invaded': self.invaded, 'distance_to_goal': self._local_planner.distance_to_goal, 'viz_img': self._viz_queue.get() if self.viz_camera else None } info.update(result) return info def _is_light_red(self, agent): lights_list = self._world.get_actors().filter('*traffic_light*') is_light_red, _ = agent._is_light_red(lights_list) return is_light_red def _setup_sensors(self): super()._setup_sensors() if self.viz_camera: viz_camera_bp = self._blueprints.find('sensor.camera.rgb') viz_camera = self._world.spawn_actor( viz_camera_bp, carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=self._player) viz_camera_bp.set_attribute('image_size_x', '640') viz_camera_bp.set_attribute('image_size_y', '480') # Set camera queues self._viz_queue = queue.Queue() viz_camera.listen(self._viz_queue.put) self._actor_dict['sensor'].append(self.viz_camera) # Setup collision test and add collision test sensor to actor dictionary self.collision_test = CollisionTest( self._player) # , terminate_on_failure=False) if self.collision_test._collision_sensor_test: self._actor_dict['sensor'].append( self.collision_test._collision_sensor_test) def render_world(self): import matplotlib.pyplot as plt from matplotlib.patches import Circle plt.clf() plt.tight_layout() plt.axis('off') fig, ax = plt.subplots(1, 1) ax.get_xaxis().set_visible(False) ax.get_yaxis().set_visible(False) world = super().render_world() world[np.all(world == (255, 255, 255), axis=-1)] = [100, 100, 100] world[np.all(world == (0, 0, 0), axis=-1)] = [255, 255, 255] ax.imshow(world) prev_command = -1 for i, (node, command) in enumerate(self._local_planner._route): command = int(command) pixel_x, pixel_y = self.world_to_pixel(node.transform.location) if command != prev_command and prev_command != -1: _command = {1: 'L', 2: 'R', 3: 'S', 4: 'F'}.get(command, '???') ax.text(pixel_x, pixel_y, _command, fontsize=8, color='black') ax.add_patch(Circle((pixel_x, pixel_y), 5, color='black')) elif i == 0 or i == len(self._local_planner._route) - 1: text = 'start' if i == 0 else 'end' ax.text(pixel_x, pixel_y, text, fontsize=8, color='blue') ax.add_patch(Circle((pixel_x, pixel_y), 5, color='blue')) elif i % (len(self._local_planner._route) // 10) == 0: ax.add_patch(Circle((pixel_x, pixel_y), 3, color='red')) prev_command = int(command) fig.canvas.draw() w, h = fig.canvas.get_width_height() return np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8).reshape(h, w, 3)
class PointGoalSuite(BaseSuite): def __init__( self, success_dist=5.0, col_is_failure=False, viz_camera=False, planner="new", poses_txt="", **kwargs ): super().__init__(**kwargs) self.success_dist = success_dist self.col_is_failure = col_is_failure self.planner = planner self.poses = from_file(poses_txt) self.command = RoadOption.LANEFOLLOW self.timestamp_active = 0 self._timeout = float("inf") self.viz_camera = viz_camera self._viz_queue = None self.crop_sky = kwargs["crop_sky"] def init(self, target=1, **kwargs): self._target_pose = self._map.get_spawn_points()[target] super().init(**kwargs) def ready(self): # print (self.planner) if self.planner == "new": self._local_planner = LocalPlannerNew( self._player, resolution=2.5, threshold_before=25.0, threshold_after=1.5 ) else: self._local_planner = LocalPlannerOld(self._player) self._local_planner.set_route(self._start_pose.location, self._target_pose.location) self._timeout = self._local_planner.calculate_timeout() return super().ready() def tick(self): result = super().tick() self._local_planner.run_step() self.command = self._local_planner.checkpoint[1] self.node = self._local_planner.checkpoint[0].transform.location self._next = self._local_planner.target[0].transform.location return result def get_observations(self): result = dict() result.update(super().get_observations()) result["command"] = int(self.command) result["node"] = np.array([self.node.x, self.node.y]) result["next"] = np.array([self._next.x, self._next.y]) return result @property def weathers(self): return self._weathers @property def pose_tasks(self): return self.poses def clean_up(self): super().clean_up() self.timestamp_active = 0 self._timeout = float("inf") self._local_planner = None # Clean-up cameras if self._viz_queue: with self._viz_queue.mutex: self._viz_queue.queue.clear() def is_failure(self): if self.timestamp_active >= self._timeout or self._tick >= 10000: return True elif self.col_is_failure and self.collided: return True return False def is_success(self): location = self._player.get_location() distance = location.distance(self._target_pose.location) return distance <= self.success_dist def apply_control(self, control): result = super().apply_control(control) # is_light_red = self._is_light_red(agent) # # if is_light_red: # self.timestamp_active -= 1 self.timestamp_active += 1 # Return diagnostics location = self._player.get_location() orientation = self._player.get_transform().get_forward_vector() velocity = self._player.get_velocity() speed = np.linalg.norm([velocity.x, velocity.y, velocity.z]) info = { "x": location.x, "y": location.y, "z": location.z, "ori_x": orientation.x, "ori_y": orientation.y, "speed": speed, "collided": self.collided, "invaded": self.invaded, "distance_to_goal": self._local_planner.distance_to_goal, "viz_img": self._viz_queue.get() if self.viz_camera else None, } info.update(result) return info def _is_light_red(self, agent): lights_list = self._world.get_actors().filter("*traffic_light*") is_light_red, _ = agent._is_light_red(lights_list) return is_light_red def _setup_sensors(self): super()._setup_sensors() if self.viz_camera: viz_camera_bp = self._blueprints.find("sensor.camera.rgb") viz_camera = self._world.spawn_actor( viz_camera_bp, carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), attach_to=self._player, ) viz_camera_bp.set_attribute("image_size_x", "640") viz_camera_bp.set_attribute("image_size_y", "480") # Set camera queues self._viz_queue = queue.Queue() viz_camera.listen(self._viz_queue.put) self._actor_dict["sensor"].append(viz_camera) def render_world(self): import matplotlib.pyplot as plt from matplotlib.patches import Circle plt.clf() plt.tight_layout() plt.axis("off") fig, ax = plt.subplots(1, 1) ax.get_xaxis().set_visible(False) ax.get_yaxis().set_visible(False) world = super().render_world() world[np.all(world == (255, 255, 255), axis=-1)] = [100, 100, 100] world[np.all(world == (0, 0, 0), axis=-1)] = [255, 255, 255] ax.imshow(world) prev_command = -1 for i, (node, command) in enumerate(self._local_planner._route): command = int(command) pixel_x, pixel_y = self.world_to_pixel(node.transform.location) if command != prev_command and prev_command != -1: _command = {1: "L", 2: "R", 3: "S", 4: "F"}.get(command, "???") ax.text(pixel_x, pixel_y, _command, fontsize=8, color="black") ax.add_patch(Circle((pixel_x, pixel_y), 5, color="black")) elif i == 0 or i == len(self._local_planner._route) - 1: text = "start" if i == 0 else "end" ax.text(pixel_x, pixel_y, text, fontsize=8, color="blue") ax.add_patch(Circle((pixel_x, pixel_y), 5, color="blue")) elif i % (len(self._local_planner._route) // 10) == 0: ax.add_patch(Circle((pixel_x, pixel_y), 3, color="red")) prev_command = int(command) fig.canvas.draw() w, h = fig.canvas.get_width_height() return np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8).reshape(h, w, 3)