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, 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=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, **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 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=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.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, 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().__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, vehicle: Vehicle, agent_settings: AgentConfig): super().__init__(vehicle, agent_settings) self.controller = PurePursuitController(agent=self) self.mission_planner = JSONWaypointPlanner(self) self.behavior_planner = BehaviorPlanner(self) self.local_planner = SimpleWaypointFollowingLocalPlanner(agent=self, mission_planner=self.mission_planner, behavior_planner=self.behavior_planner, controller=self.controller) self.logger.debug("JSON Waypoint Agent Initialized")
def __init__(self, agent, controller: Optional[Controller] = None, behavior_planner: Optional[BehaviorPlanner] = None, mission_planner: Optional[MissionPlanner] = None, **kwargs): super().__init__(agent=agent, **kwargs) self.controller = (Controller( agent=agent) if controller is None else controller) self.behavior_planner = (BehaviorPlanner( agent=agent) if behavior_planner is None else behavior_planner) self.mission_planner = (MissionPlanner( agent=agent) if mission_planner is None else mission_planner) self.way_points_queue = deque()
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.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()}")
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