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
Beispiel #2
0
    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
Beispiel #3
0
 def _read_data_file(self):
     raw = json.load(fp=self.file_path.open('r'))
     result = [MapEntry.parse_obj(i) for i in raw]
     return result