class PIDAgent(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=(-10, 10))
self.mission_planner = WaypointFollowingMissionPlanner(agent=self)
# initiated right after mission plan
self.behavior_planner = BehaviorPlanner(agent=self)
self.local_planner = SmoothWaypointFollowingLocalPlanner(
agent=self,
controller=self.pid_controller,
mission_planner=self.mission_planner,
behavior_planner=self.behavior_planner,
closeness_threshold=1)
self.lane_detector = LaneDetector(agent=self)
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(PIDAgent, self).run_step(vehicle=vehicle,
sensors_data=sensors_data)
self.transform_history.append(self.vehicle.transform)
self.lane_detector.run_in_series()
if self.local_planner.is_done():
control = VehicleControl()
self.logger.debug("Path Following Agent is Done. Idling.")
else:
control = self.local_planner.run_in_series()
return control
class LaneDetectionAgent(Agent):
def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
super().__init__(vehicle, agent_settings, **kwargs)
self.lane_detector = LaneDetector(agent=self)
def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
super().run_step(sensors_data=sensors_data, vehicle=vehicle)
try:
self.lane_detector.run_in_series()
except Exception as e:
self.logger.error(e)
return VehicleControl()
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=(-10, 10))
self.mission_planner = WaypointFollowingMissionPlanner(agent=self)
# initiated right after mission plan
self.behavior_planner = BehaviorPlanner(agent=self)
self.local_planner = SmoothWaypointFollowingLocalPlanner(
agent=self,
controller=self.pid_controller,
mission_planner=self.mission_planner,
behavior_planner=self.behavior_planner,
closeness_threshold=1)
self.lane_detector = LaneDetector(agent=self)
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)
# 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, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
super().__init__(vehicle, agent_settings, **kwargs)
self.lane_detector = LaneDetector(agent=self)