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)
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, target_speed=120, **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=(-1, 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.local_planner = SimpleWaypointFollowingLocalPlanner( # agent=self, # controller=self.pid_controller, # mission_planner=self.mission_planner, # behavior_planner=self.behavior_planner, # closeness_threshold=1) self.logger.debug(f"Waypoint Following Agent Initiated. Reading f" f"rom {self.route_file_path.as_posix()}")
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
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"
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(PointCloudMapRecordingAgent, self).__init__(**kwargs) self.logger.debug("GPD2 Agent Initialized") self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.controller = \ self.pid_controller = VehiclePIDController(agent=self, args_lateral=PIDParam.default_lateral_param(), args_longitudinal=PIDParam.default_longitudinal_param(), target_speed=20) 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.ground_plane_point_cloud_detector = GroundPlanePointCloudDetector( agent=self, max_points_to_convert=20000, ground_tilt_threshhold=0.05) self.visualizer = Visualizer(agent=self) self.map_history: List[MapEntry] = [] self.file_written = False
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, target_speed=40, **kwargs): super().__init__(**kwargs) # self.target_speed = target_speed # ROAR Academy: Original Code by Michael self.target_speed = self.agent_settings.target_speed # ROAR Academy 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)) # ROAR Academy: Original Code by Michael self.pid_controller = PIDController( agent=self, steering_boundary=self.agent_settings.steering_boundary, throttle_boundary=self.agent_settings.throttle_boundary ) # ROAR Academy # ROAR Academy: pwm signals 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.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 = 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, target_speed=50): super().__init__(vehicle=vehicle, agent_settings=agent_settings) self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pure_pursuit_controller = \ PurePursuitController(agent=self, target_speed=target_speed, look_ahead_gain=0.1, look_ahead_distance=3) 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.pure_pursuit_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=3)
def __init__(self, **kwargs): super().__init__(**kwargs) self.ground_plane_detector = GroundPlaneDetector(agent=self) self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pid_controller = PurePursuitController(agent=self, target_speed=40) self.mission_planner = WaypointFollowingMissionPlanner(agent=self) # initiated right after mission plan self.behavior_planner = BehaviorPlanner(vehicle=self.vehicle) 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(self) self.map_history: List[MapEntry] = [] self.file_written = False
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)
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()}")
def __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.logger = logging.getLogger("PathFollowingAgent") self.mpc_controller = VehicleMPCController( agent=self, route_file_path=Path(self.agent_settings.waypoint_file_path), 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.mpc_controller, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, closeness_threshold=1) self.visualizer = Visualizer(agent=self) self.logger.debug(f"Waypoint Following Agent Initiated. " f"Reading from " f"{self.agent_settings.waypoint_file_path}") self.curr_max_err = 0 self.counter = 0 self.total_err = 0
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 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