class OccupancyMapAgent(Agent): def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs): super().__init__(vehicle, agent_settings, **kwargs) self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pid_controller = PIDController(agent=self, steering_boundary=(-1, 1), throttle_boundary=(0, 1)) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = SimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1.5) self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=550, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=5000) self.obstacle_from_depth_detector = ObstacleFromDepth( agent=self, threaded=True, max_detectable_distance=0.3, max_points_to_convert=10000, min_obstacle_height=2) self.add_threaded_module(self.obstacle_from_depth_detector) self.vis = o3d.visualization.Visualizer() self.vis.create_window(width=500, height=500) self.pcd = o3d.geometry.PointCloud() self.points_added = False def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super().run_step(sensors_data=sensors_data, vehicle=vehicle) control = self.local_planner.run_in_series() option = "obstacle_coords" # ground_coords, point_cloud_obstacle_from_depth if self.kwargs.get(option, None) is not None: # print("curr_transform", self.vehicle.transform) points = self.kwargs[option] self.occupancy_map.update(points) self.occupancy_map.visualize() if self.points_added is False: self.pcd = o3d.geometry.PointCloud() point_means = np.mean(points, axis=0) self.pcd.points = o3d.utility.Vector3dVector(points - point_means) self.vis.add_geometry(self.pcd) self.vis.poll_events() self.vis.update_renderer() self.points_added = True else: point_means = np.mean(points, axis=0) self.pcd.points = o3d.utility.Vector3dVector(points - point_means) self.vis.update_geometry(self.pcd) self.vis.poll_events() self.vis.update_renderer() return control
class OccupancyMapAgent(Agent): def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs): super().__init__(vehicle, agent_settings, **kwargs) self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pid_controller = PIDController(agent=self, steering_boundary=(-1, 1), throttle_boundary=(0, 1)) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = SimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1) self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=2000, world_coord_resolution=10, occu_prob=0.9) # 1 m = 100 cm self.add_threaded_module( DepthToPointCloudDetector(agent=self, should_compute_global_pointcloud=True, threaded=True, scale_factor=1000)) # self.gpd = GroundPlaneDetector(self, threaded=True) # self.add_threaded_module(self.gpd) self.obstacle_detector = ObstacleDetector(self, threaded=True) self.add_threaded_module(self.obstacle_detector) # self.vis = o3d.visualization.Visualizer() # self.vis.create_window(width=500, height=500) # self.pcd = o3d.geometry.PointCloud() # self.points_added = False def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super().run_step(sensors_data=sensors_data, vehicle=vehicle) control = self.local_planner.run_in_series() if self.kwargs.get("obstacle_coords", None) is not None: points = self.kwargs["obstacle_coords"] self.occupancy_map.update(points) self.occupancy_map.visualize(self.vehicle.transform.location) # print(self.vehicle.transform) # cv2.imshow("mask", self.obstacle_detector.curr_mask) # cv2.waitKey(1) # # self.occupancy_map.visualize() # if self.points_added is False: # self.pcd = o3d.geometry.PointCloud() # self.pcd.points = o3d.utility.Vector3dVector(grounds) # self.vis.add_geometry(self.pcd) # self.vis.poll_events() # self.vis.update_renderer() # self.points_added = True # else: # self.pcd.points = o3d.utility.Vector3dVector(grounds) # self.vis.update_geometry(self.pcd) # self.vis.poll_events() # self.vis.update_renderer() return control
class RecordingAgent(Agent): def __init__(self, target_speed=20, **kwargs): super().__init__(**kwargs) # ensure recording status is ON self.agent_settings.save_sensor_data = True super().__init__(**kwargs) self.target_speed = target_speed self.logger = logging.getLogger("Recording Agent") self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pid_controller = PIDController(agent=self, steering_boundary=(-1, 1), throttle_boundary=(0, 1), target_speed=target_speed) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = LoopSimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1) self.occupancy_map = OccupancyGridMap( absolute_maximum_map_size=1000, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=5000, threaded=True, should_save=self.agent_settings.save_sensor_data) self.obstacle_from_depth_detector = ObstacleFromDepth( agent=self, threaded=True, max_detectable_distance=0.3, max_points_to_convert=10000, min_obstacle_height=2) self.add_threaded_module(self.obstacle_from_depth_detector) self.add_threaded_module(self.occupancy_map) self.option = "obstacle_coords" self.lap_count = 0 def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super(RecordingAgent, self).run_step(sensors_data=sensors_data, vehicle=vehicle) self.transform_history.append(self.vehicle.transform) control = self.local_planner.run_in_series() if self.kwargs.get(self.option, None) is not None: points = self.kwargs[self.option] self.occupancy_map.update_async(points) self.occupancy_map.visualize(transform=self.vehicle.transform, view_size=(200, 200)) return control
class PointCloudAgent(Agent): def __init__(self, **kwargs): super(PointCloudAgent, self).__init__(**kwargs) self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.controller = PurePursuitController(agent=self, target_speed=20) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = SimpleWaypointFollowingLocalPlanner( agent=self, controller=self.controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1) """ self.gp_pointcloud_detector = GroundPlanePointCloudDetector(agent=self, max_points_to_convert=10000, nb_neighbors=100, std_ratio=1) """ self.gp_pointcloud_detector = GroundPlanePointCloudDetector( agent=self, max_points_to_convert=10000, nb_neighbors=100, std_ratio=1) self.occupancy_grid_map = OccupancyGridMap( absolute_maximum_map_size=800) # self.visualizer = Visualizer(agent=self) def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super(PointCloudAgent, self).run_step(sensors_data, vehicle) try: self.local_planner.run_in_series() points = self.gp_pointcloud_detector.run_in_series() # (N x 3) self.occupancy_grid_map.update_grid_map_from_world_cord( world_cords_xy=points[:, :2]) self.occupancy_grid_map.visualize( vehicle_location=self.vehicle.transform.location) # print(np.amin(points, axis=0), np.amax(points, axis=0), self.vehicle.transform.location.to_array()) # pcd = o3d.geometry.PointCloud() # pcd.points = o3d.utility.Vector3dVector(points) # o3d.visualization.draw_geometries([pcd]) # self.occupancy_grid_map.update_grid_map_from_world_cord(points[:, :2]) # self.occupancy_grid_map.visualize(vehicle_location=self.vehicle.transform.location) except Exception as e: self.logger.error(f"Point cloud RunStep Error: {e}") finally: return self.local_planner.run_in_series()
class OccuMapDemoDrivingAgent(Agent): def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs): super().__init__(vehicle, agent_settings, **kwargs) self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=550, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=5000) occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy") self.occupancy_map.load_from_file(file_path=occu_map_file_path) def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super(OccuMapDemoDrivingAgent, self).run_step(sensors_data=sensors_data, vehicle=vehicle) self.occupancy_map.visualize(transform=self.vehicle.transform, view_size=(200, 200)) return VehicleControl()
class MarkAgent(Agent): def __init__(self, **kwargs): super().__init__(**kwargs) self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pid_controller = PIDController(agent=self, steering_boundary=(-1, 1), throttle_boundary=(0, 1)) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = DynamicWindowsApproach( agent=self, controller=self.pid_controller) occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy") self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=550, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=5000, threaded=True) self.occupancy_map.load_from_file(file_path=occu_map_file_path) # self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self, # threaded=True, # max_detectable_distance=0.3, # max_points_to_convert=10000, # min_obstacle_height=2) # self.add_threaded_module(self.obstacle_from_depth_detector) # self.add_threaded_module(self.occupancy_map) def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super(MarkAgent, self).run_step(vehicle=vehicle, sensors_data=sensors_data) control = self.local_planner.run_in_series() option = "obstacle_coords" # ground_coords, point_cloud_obstacle_from_depth if self.kwargs.get(option, None) is not None: points = self.kwargs[option] self.occupancy_map.update_async(points) self.occupancy_map.visualize() self.occupancy_map.get_map() return control
class FreeSpaceAutoAgent(Agent): def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs): super().__init__(vehicle, agent_settings, **kwargs) # initialize occupancy grid map content self.occu_map = OccupancyGridMap(agent=self) self.depth_to_pcd = DepthToPointCloudDetector(agent=self) self.ground_plane_detector = GroundPlaneDetector(agent=self) self.lane_detector = LaneDetector(agent=self) # initialize open3d related content self.vis = o3d.visualization.Visualizer() self.vis.create_window(width=500, height=500) self.pcd = o3d.geometry.PointCloud() self.coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame( ) self.points_added = False def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super(FreeSpaceAutoAgent, self).run_step(sensors_data, vehicle) if self.front_depth_camera.data is not None and self.front_rgb_camera.data is not None: pcd = self.depth_to_pcd.run_in_series() points: np.ndarray = np.asarray(pcd.points) colors: np.ndarray = np.asarray(pcd.colors) ground_locs = np.where(points[:, 1] < np.mean(points[:, 1])) points = points[ground_locs] colors = colors[ground_locs] pcd.points = o3d.utility.Vector3dVector(points) pcd.colors = o3d.utility.Vector3dVector(colors) # self.logger.info(f"{self.vehicle.transform}") self.occu_map.update(points) self.occu_map.visualize() self.non_blocking_pcd_visualization(pcd=pcd, should_center=True, should_show_axis=True, axis_size=1) # self.pcd = pcd return VehicleControl() def non_blocking_pcd_visualization(self, pcd: o3d.geometry.PointCloud, should_center=False, should_show_axis=False, axis_size: float = 0.1): points = np.asarray(pcd.points) colors = np.asarray(pcd.colors) if should_center: points = points - np.mean(points, axis=0) if self.points_added is False: self.pcd = o3d.geometry.PointCloud() self.pcd.points = o3d.utility.Vector3dVector(points) self.pcd.colors = o3d.utility.Vector3dVector(colors) if should_show_axis: self.coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame( size=axis_size, origin=np.mean(points, axis=0)) self.vis.add_geometry(self.coordinate_frame) self.vis.add_geometry(self.pcd) self.points_added = True else: # print(np.shape(np.vstack((np.asarray(self.pcd.points), points)))) self.pcd.points = o3d.utility.Vector3dVector(points) self.pcd.colors = o3d.utility.Vector3dVector(colors) if should_show_axis: self.coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame( size=axis_size, origin=np.mean(points, axis=0)) self.vis.update_geometry(self.coordinate_frame) self.vis.update_geometry(self.pcd) self.vis.poll_events() self.vis.update_renderer()