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