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()
