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 PIDAgent(Agent): def __init__(self, target_speed=40, **kwargs): super().__init__(**kwargs) self.target_speed = target_speed self.logger = logging.getLogger("PID Agent") self.route_file_path = Path(self.agent_settings.waypoint_file_path) self.pid_controller = PIDController(agent=self, steering_boundary=(-1, 1), throttle_boundary=(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.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) if self.is_done: control = VehicleControl() self.logger.debug("Path Following Agent is Done. Idling.") else: control = self.local_planner.run_in_series() 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
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 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_in_series()
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 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 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_in_series() return next_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_in_series() 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_in_series() control = self.local_planner.run_in_series() 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_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 ]
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