class PotentialFieldAgent(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.occupancy_map = OccupancyGridMap(agent=self, threaded=True) self.depth_to_obstacle = ObstacleFromDepth(agent=self, threaded=True) self.add_threaded_module(self.occupancy_map) self.add_threaded_module(self.depth_to_obstacle) # occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy") # self.occupancy_map.load_from_file(occu_map_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 = PotentialFieldPlanner( agent=self, behavior_planner=self.behavior_planner, mission_planner=self.mission_planner, controller=self.pid_controller) def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super().run_step(sensors_data=sensors_data, vehicle=vehicle) if self.kwargs.get("obstacle_coords") is not None: self.occupancy_map.update_async(self.kwargs.get("obstacle_coords")) control = self.local_planner.run_in_series() return control
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 __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 = 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=1500, 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"
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 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
def __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.target_speed = target_speed self.logger = logging.getLogger("PID 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)) 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(agent=self, threaded=True) self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self, threaded=True) self.add_threaded_module(self.obstacle_from_depth_detector) self.add_threaded_module(self.occupancy_map) self.logger.debug(f"Waypoint Following Agent Initiated. Reading f" f"rom {self.route_file_path.as_posix()}")
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)
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 = DWAPlanner(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) 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 __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)
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)
class RLLocalPlannerAgent(Agent): def __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.target_speed = target_speed self.logger = logging.getLogger("PID 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)) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = RLLocalPlanner(agent=self, controller=self.pid_controller) self.traditional_local_planner = SimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1.5) self.absolute_maximum_map_size, self.map_padding = 1000, 40 self.occupancy_map = OccupancyGridMap( absolute_maximum_map_size=self.absolute_maximum_map_size, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=10000, threaded=True) self.obstacle_from_depth_detector = ObstacleFromDepth( agent=self, threaded=True, max_detectable_distance=0.5, max_points_to_convert=20000, min_obstacle_height=2) self.add_threaded_module(self.obstacle_from_depth_detector) # self.add_threaded_module(self.occupancy_map) self.logger.debug(f"Waypoint Following Agent Initiated. Reading f" f"rom {self.route_file_path.as_posix()}") def run_step(self, vehicle: Vehicle, sensors_data: SensorsData) -> VehicleControl: super(RLLocalPlannerAgent, self).run_step(vehicle=vehicle, sensors_data=sensors_data) self.traditional_local_planner.run_in_series() self.transform_history.append(self.vehicle.transform) if self.is_done: # will never enter here control = VehicleControl() self.logger.debug("Path Following Agent is Done. Idling.") else: 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(points) control = self.local_planner.run_in_series() 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 RLLocalPlannerAgent(Agent): def __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.target_speed = target_speed self.logger = logging.getLogger("PID 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)) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = RLLocalPlanner(agent=self, controller=self.pid_controller) self.traditional_local_planner = SimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1.5) self.absolute_maximum_map_size, self.map_padding = 1000, 40 self.occupancy_map = OccupancyGridMap(agent=self, threaded=True) self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self, threaded=True) self.add_threaded_module(self.obstacle_from_depth_detector) # self.add_threaded_module(self.occupancy_map) self.logger.debug(f"Waypoint Following Agent Initiated. Reading f" f"rom {self.route_file_path.as_posix()}") def run_step(self, vehicle: Vehicle, sensors_data: SensorsData) -> VehicleControl: super(RLLocalPlannerAgent, self).run_step(vehicle=vehicle, sensors_data=sensors_data) self.traditional_local_planner.run_in_series() self.transform_history.append(self.vehicle.transform) 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(points) control = self.local_planner.run_in_series() return control def get_obs(self): ch1 = self.occupancy_map.get_map(transform=self.vehicle.transform, view_size=(100, 100)) ch1 = np.expand_dims((ch1 * 255).astype(np.uint8), -1) ch2 = np.zeros(shape=(100, 100, 1)) ch3 = np.zeros(shape=ch2.shape) obs = np.concatenate([ch1, ch2, ch3], axis=2) print(np.shape(obs)) return obs
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()
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 __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.logger = logging.getLogger("PID Agent") self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pid_controller = VehiclePIDController( agent=self, args_lateral=PIDParam.default_lateral_param(), args_longitudinal=PIDParam.default_longitudinal_param(), target_speed=target_speed) 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.visualizer = Visualizer(agent=self) self.occupancy_grid_map = OccupancyGridMap(absolute_maximum_map_size=800) self.logger.debug( f"Waypoint Following Agent Initiated. Reading f" f"rom {self.route_file_path.as_posix()}") self.curr_max_err = 0 self.counter = 0 self.total_err = 0
class RLOccuMapE2ETrainingAgent(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.traditional_local_planner = LoopSimpleWaypointFollowingLocalPlanner( 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=1000, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=10000, threaded=True) self.obstacle_from_depth_detector = ObstacleFromDepth( agent=self, threaded=True, max_detectable_distance=0.5, max_points_to_convert=20000, 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(RLOccuMapE2ETrainingAgent, self).run_step(sensors_data, vehicle) self.traditional_local_planner.run_in_series() self.transform_history.append(self.vehicle.transform) 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) if self.kwargs.get("control") is None: return VehicleControl() else: return self.kwargs.get("control")
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)
class OccuDebugAgent(Agent): def __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.target_speed = target_speed self.logger = logging.getLogger("PID 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)) 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(agent=self, threaded=True) self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self, threaded=True) self.add_threaded_module(self.obstacle_from_depth_detector) self.add_threaded_module(self.occupancy_map) self.logger.debug(f"Waypoint Following Agent Initiated. Reading f" f"rom {self.route_file_path.as_posix()}") def run_step(self, vehicle: Vehicle, sensors_data: SensorsData) -> VehicleControl: super(OccuDebugAgent, self).run_step(vehicle=vehicle, sensors_data=sensors_data) self.transform_history.append(self.vehicle.transform) 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) control = self.kwargs.get("control", VehicleControl()) return control
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 = LoopSimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1.5) # the part about visualization self.occupancy_map = OccupancyGridMap(agent=self, threaded=True) occ_file_path = Path( "../ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy") self.occupancy_map.load_from_file(occ_file_path) self.plan_lst = list( self.mission_planner.produce_single_lap_mission_plan()) self.kwargs = kwargs self.interval = self.kwargs.get('interval', 50) self.look_back = self.kwargs.get('look_back', 5) self.look_back_max = self.kwargs.get('look_back_max', 10) self.thres = self.kwargs.get('thres', 1e-3) self.int_counter = 0 self.counter = 0 self.finished = False self.curr_dist_to_strip = 0 self.bbox: Optional[LineBBox] = None self._get_next_bbox()
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.occupancy_map = OccupancyGridMap(agent=self, threaded=True) self.depth_to_obstacle = ObstacleFromDepth(agent=self, threaded=True) self.add_threaded_module(self.occupancy_map) self.add_threaded_module(self.depth_to_obstacle) # occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy") # self.occupancy_map.load_from_file(occu_map_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 = PotentialFieldPlanner( agent=self, behavior_planner=self.behavior_planner, mission_planner=self.mission_planner, controller=self.pid_controller)
def __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.target_speed = target_speed self.logger = logging.getLogger("PID 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)) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(agent=self) self.local_planner = RLLocalPlanner(agent=self, controller=self.pid_controller) self.traditional_local_planner = SimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1.5) self.absolute_maximum_map_size, self.map_padding = 1000, 40 self.occupancy_map = OccupancyGridMap( absolute_maximum_map_size=self.absolute_maximum_map_size, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=10000, threaded=True) self.obstacle_from_depth_detector = ObstacleFromDepth( agent=self, threaded=True, max_detectable_distance=0.5, max_points_to_convert=20000, min_obstacle_height=2) self.add_threaded_module(self.obstacle_from_depth_detector) # self.add_threaded_module(self.occupancy_map) self.logger.debug(f"Waypoint Following Agent Initiated. Reading f" f"rom {self.route_file_path.as_posix()}")
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 RLDepthE2EAgent(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 = LoopSimpleWaypointFollowingLocalPlanner( agent=self, controller=self.pid_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1.5) # the part about visualization self.occupancy_map = OccupancyGridMap(agent=self, threaded=True) occ_file_path = Path( "../ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy") self.occupancy_map.load_from_file(occ_file_path) self.plan_lst = list( self.mission_planner.produce_single_lap_mission_plan()) self.kwargs = kwargs self.interval = self.kwargs.get('interval', 50) self.look_back = self.kwargs.get('look_back', 5) self.look_back_max = self.kwargs.get('look_back_max', 10) self.thres = self.kwargs.get('thres', 1e-3) self.int_counter = 0 self.counter = 0 self.finished = False self.curr_dist_to_strip = 0 self.bbox: Optional[LineBBox] = None self._get_next_bbox() def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super(RLDepthE2EAgent, self).run_step(sensors_data, vehicle) self.local_planner.run_in_series() _, self.curr_dist_to_strip = self.bbox_step() if self.kwargs.get("control") is None: return VehicleControl() else: return self.kwargs.get("control") def bbox_step(self): """ This is the function that the line detection agent used Main function to use for detecting whether the vehicle reached a new strip in the current step. The old strip (represented as a bbox) will be gone forever return: crossed: a boolean value indicating whether a new strip is reached dist (optional): distance to the strip, value no specific meaning """ self.counter += 1 if not self.finished: crossed, dist = self.bbox.has_crossed(self.vehicle.transform) if crossed: self.int_counter += 1 self._get_next_bbox() return crossed, dist return False, 0.0 def _get_next_bbox(self): # make sure no index out of bound error curr_lb = self.look_back curr_idx = self.int_counter * self.interval while curr_idx + curr_lb < len(self.plan_lst): if curr_lb > self.look_back_max: self.int_counter += 1 curr_lb = self.look_back curr_idx = self.int_counter * self.interval continue t1 = self.plan_lst[curr_idx] t2 = self.plan_lst[curr_idx + curr_lb] dx = t2.location.x - t1.location.x dz = t2.location.z - t1.location.z if abs(dx) < self.thres and abs(dz) < self.thres: curr_lb += 1 else: self.bbox = LineBBox(t1, t2) return # no next bbox print("finished all the iterations!") self.finished = True
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=800, world_coord_resolution=1, occu_prob=0.99, max_points_to_convert=10000, threaded=True) self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self, threaded=True, max_detectable_distance=0.5, max_points_to_convert=20000, min_obstacle_height=2) self.add_threaded_module(self.obstacle_from_depth_detector) self.add_threaded_module(self.occupancy_map) # 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: points = self.kwargs[option] self.occupancy_map.update_async(points) arb_points = [self.local_planner.way_points_queue[0].location] m = self.occupancy_map.get_map(transform=self.vehicle.transform, view_size=(200, 200), vehicle_value=-1, arbitrary_locations=arb_points, arbitrary_point_value=-5) # print(np.where(m == -5)) # cv2.imshow("m", m) # cv2.waitKey(1) # occu_map_vehicle_center = np.array(list(zip(*np.where(m == np.min(m))))[0]) # correct_next_waypoint_world = self.local_planner.way_points_queue[0] # diff = np.array([correct_next_waypoint_world.location.x, # correct_next_waypoint_world.location.z]) - \ # np.array([self.vehicle.transform.location.x, # self.vehicle.transform.location.z]) # correct_next_waypoint_occu = occu_map_vehicle_center + diff # correct_next_waypoint_occu = np.array([49.97, 44.72596359]) # estimated_world_coord = self.occupancy_map.cropped_occu_to_world( # cropped_occu_coord=correct_next_waypoint_occu, vehicle_transform=self.vehicle.transform, # occu_vehicle_center=occu_map_vehicle_center) # print(f"correct o-> {correct_next_waypoint_occu}" # f"correct w-> {correct_next_waypoint_world.location} | " # f"estimated = {estimated_world_coord.location.x}") # cv2.imshow("m", m) # cv2.waitKey(1) # print() # 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() if self.local_planner.is_done(): self.mission_planner.restart() self.local_planner.restart() 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()