def __init__(self, target_speed=120, **kwargs):
super().__init__(**kwargs)
self.target_speed = target_speed
self.logger = logging.getLogger("Stanley 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.bstanley_controller = BStanley_controller(agent=self,
steering_boundary=(-1,
1),
throttle_boundary=(-1,
1))
#self.cstanley_controller = CStanley_controller(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 = SimpleWaypointFollowingLocalPlanner(
agent=self,
controller=self.bstanley_controller,
#controller=self.cstanley_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(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, 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()}")
class JAM1Agent(Agent):
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 = 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 run_step(self, vehicle: Vehicle,
sensors_data: SensorsData) -> VehicleControl:
super(JAM1Agent, self).run_step(vehicle=vehicle,
sensors_data=sensors_data)
self.transform_history.append(self.vehicle.transform)
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
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, **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.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=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 __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, 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")
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 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 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
class PurePursuitAgent(Agent):
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 run_step(self, sensors_data: SensorsData,
vehicle: Vehicle) -> VehicleControl:
super(PurePursuitAgent, self).run_step(sensors_data=sensors_data,
vehicle=vehicle)
return self.local_planner.run_step()
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
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
class PIDAgent(Agent):
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 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)
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 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 = 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 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)
if self.is_done:
control = VehicleControl()
self.logger.debug("Recording Agent is Done. Idling.")
else:
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 MPCAgent(Agent):
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
def run_step(self, vehicle: Vehicle,
sensors_data: SensorsData) -> VehicleControl:
super(MPCAgent, self).run_step(vehicle=vehicle,
sensors_data=sensors_data)
self.transform_history.append(self.vehicle.transform)
if self.local_planner.is_done():
control = VehicleControl()
self.logger.debug("Path Following Agent is Done. Idling.")
else:
control = self.local_planner.run_step()
return control
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 JsonWaypointAgent(Agent):
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 run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
super(JsonWaypointAgent, self).run_step(sensors_data=sensors_data, vehicle=vehicle)
next_control = self.local_planner.run_step()
return next_control
class PIDAgent(Agent):
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 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)
# print(self.vehicle.transform, self.vehicle.velocity)
if self.is_done:
control = VehicleControl()
self.logger.debug("Path Following Agent is Done. Idling.")
else:
control = self.local_planner.run_in_series()
# control.steering = 1 # ROAR Academy
# control.throttle = 1 # ROAR Academy
return control
class MapGeneratingAgentV3(Agent):
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 run_step(self, sensors_data: SensorsData,
vehicle: Vehicle) -> VehicleControl:
super(MapGeneratingAgentV3, self).run_step(sensors_data, vehicle)
self.ground_plane_detector.run_step()
control = self.local_planner.run_step()
try:
if self.ground_plane_detector.curr_segmentation is not None and \
len(self.ground_plane_detector.curr_segmentation) > 0:
seg_visual = self.ground_plane_detector.curr_segmentation.copy(
)
waypoint = self.local_planner.way_points_queue[2]
img_pos_center = self.visualizer.calculate_img_pos(
waypoint, camera=self.front_depth_camera)
detection_line = seg_visual[img_pos_center[1], :, :]
# find left obstacle position along detection line
left_obstacle_pos = np.array([0, img_pos_center[1], 1])
for x in range(img_pos_center[0], 0, -1):
if np.all(detection_line[x] ==
self.ground_plane_detector.OBSTACLE):
left_obstacle_pos[0] = x
break
# find right obstacle position along detection line
right_obstacle_pos = np.array([
np.shape(self.front_depth_camera.data)[1] - 1,
img_pos_center[1], 1
])
for x in range(img_pos_center[0],
np.shape(self.front_depth_camera.data)[1] - 1,
1):
if np.all(detection_line[x] ==
self.ground_plane_detector.OBSTACLE):
right_obstacle_pos[0] = x
break
# make visualization
seg_visual[img_pos_center[1], :, :] = [0, 0, 255]
seg_visual[left_obstacle_pos[1]:left_obstacle_pos[1] + 5,
left_obstacle_pos[0]:left_obstacle_pos[0] +
5] = [0, 255, 0]
seg_visual[img_pos_center[1] - 5:img_pos_center[1],
img_pos_center[0] -
5:img_pos_center[0]] = [0, 255, 0]
seg_visual[right_obstacle_pos[1] - 5:right_obstacle_pos[1],
right_obstacle_pos[0] -
5:right_obstacle_pos[0]] = [0, 255, 0]
cv2.imshow("seg_vis", seg_visual)
cv2.waitKey(1)
# find depth
depth = self.front_depth_camera.data
depth_center = depth[img_pos_center[1]][
img_pos_center[0]] * 1000
depth_left = depth[left_obstacle_pos[1]][
left_obstacle_pos[0]] * 1000
depth_right = depth[right_obstacle_pos[1]][
right_obstacle_pos[0]] * 1000
# reconstruct p2d and transform it back to world space
raw_p2d = np.array([[
left_obstacle_pos[0] * depth_left,
left_obstacle_pos[1] * depth_left, depth_left
],
[
img_pos_center[0] * depth_center,
img_pos_center[1] * depth_center,
depth_center
],
[
right_obstacle_pos[0] * depth_right,
right_obstacle_pos[1] * depth_right,
depth_right
]])
cords_y_minus_z_x = np.linalg.inv(
self.front_depth_camera.intrinsics_matrix) @ raw_p2d.T
cords_xyz_1 = np.vstack([
cords_y_minus_z_x[2, :], cords_y_minus_z_x[0, :],
-cords_y_minus_z_x[1, :],
np.ones((1, np.shape(cords_y_minus_z_x)[1]))
])
points: np.ndarray = self.vehicle.transform.get_matrix(
) @ self.front_depth_camera.transform.get_matrix(
) @ cords_xyz_1
points = points.T[:, :3]
# put it into the log
map_entry = MapEntry(point_a=points[0].tolist(),
point_b=points[2].tolist())
self.map_history.append(map_entry)
except Exception as e:
self.logger.error(f"Error during map making: {e}")
if self.local_planner.is_done() and self.file_written is False:
self.logger.debug("WRITING TO FILE")
output_file_path: Path = Path(
self.agent_settings.output_data_folder_path
) / "easy_map_waypoints_v3.json"
f = output_file_path.open('w')
import json
json.dump(fp=f,
obj=[map_entry.dict() for map_entry in self.map_history],
indent=2)
f.close()
self.file_written = True
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=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 PointCloudMapRecordingAgent(Agent):
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 run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
super(PointCloudMapRecordingAgent, self).run_step(sensors_data=sensors_data, vehicle=vehicle)
control = self.local_planner.run_in_series()
try:
ground_points = self.ground_plane_point_cloud_detector.run_in_series()
# print(np.shape(ground_points))
color_image = self.front_rgb_camera.data.copy()
ground_cords_in_2d: np.ndarray = self.visualizer.world_to_img_transform(xyz=ground_points)[:, :2]
# this is a hack, without 5000 threshold, it sometimes have false detection
# if np.shape(ground_cords_in_2d)[0] > 4000:
# estimate left = (x_min, img_pos[1]) and right = (x_max, img_pos[1])
img_positions = self.visualizer.world_to_img_transform(
np.array([self.local_planner.way_points_queue[1].location.to_array()]))
img_pos = img_positions[0]
y_range = img_pos[1] - 5, img_pos[1] + 5
indices = np.where(
np.logical_and(ground_cords_in_2d[:, 1] >= y_range[0], ground_cords_in_2d[:, 1] <= y_range[1]))
bar_cords = ground_cords_in_2d[indices]
x_min, y_min = np.amin(bar_cords, axis=0)
x_max, y_max = np.amax(bar_cords, axis=0)
left_img_cord, right_img_cord = (x_min, img_pos[1]), (x_max, img_pos[1])
pts = self.img_cords_to_world_cords(left_img_cord, right_img_cord)
# save it
self.map_history.append(MapEntry(point_a=pts[0].tolist(), point_b=pts[1].tolist()))
# visualize
color_image[ground_cords_in_2d[:, 1], ground_cords_in_2d[:, 0]] = [255, 255, 255]
for y, x, _ in img_positions:
color_image[x - 2: x + 2, y - 2:y + 2] = self.visualizer.GREEN
image = cv2.line(color_image, left_img_cord, right_img_cord, (0, 255, 0), 5)
cv2.imshow("color", image)
cv2.waitKey(1)
except Exception as e:
self.logger.error(e)
# write it to file
if self.local_planner.is_done() and self.file_written is False:
self.logger.debug("WRITING TO FILE")
output_file_path: Path = Path(
self.agent_settings.output_data_folder_path) / "easy_map_waypoints_pointcloud_v3.json"
f = output_file_path.open('w')
import json
json.dump(fp=f, obj=[map_entry.dict() for map_entry in self.map_history], indent=2)
f.close()
self.file_written = True
return control
def img_cords_to_world_cords(self, left_img_cord, right_img_cord):
depth = self.front_depth_camera.data
# depth_center = depth[img_pos_center[1]][img_pos_center[0]] * 1000
depth_left = depth[left_img_cord[1]][left_img_cord[0]] * 1000
depth_right = depth[right_img_cord[1]][right_img_cord[0]] * 1000
# reconstruct p2d and transform it back to world space
raw_p2d = np.array([
[left_img_cord[0] * depth_left, left_img_cord[1] * depth_left, depth_left],
# [right_img_cord[0] * depth_center, right_img_cord[1] * depth_center, depth_center],
[right_img_cord[0] * depth_right, right_img_cord[1] * depth_right, depth_right]
])
cords_y_minus_z_x = np.linalg.inv(self.front_depth_camera.intrinsics_matrix) @ raw_p2d.T
cords_xyz_1 = np.vstack([
cords_y_minus_z_x[2, :],
cords_y_minus_z_x[0, :],
-cords_y_minus_z_x[1, :],
np.ones((1, np.shape(cords_y_minus_z_x)[1]))
])
points: np.ndarray = self.vehicle.transform.get_matrix() @ self.front_depth_camera.transform.get_matrix() @ cords_xyz_1
points = points.T[:, :3]
return points
@staticmethod
def _pix2xyz(depth_img, i, j):
return [
depth_img[i, j] * j * 1000,
depth_img[i, j] * i * 1000,
depth_img[i, j] * 1000
]
