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 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_step()
points = self.gp_pointcloud_detector.run_step() # (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_step()
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.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_step()
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
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 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_step()
try:
ground_points = self.ground_plane_point_cloud_detector.run_step()
# 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
]