1. 개요
PCD로 작성된 3차원 맵으로 부터 기존 navigation 모듈 사용을 위한 2차원의 이미지 pgm파일과 yaml 파일로 만들어 본다. pcd파일에서 직접 가능한 방법?이 있을 것 같긴 한데 추후에 알아 보기로 하고 일단 이미지로 변경하는 방법을 시도해 본다..
2. 지난번 포스트에서 gazebo 플러인으로 작성한 turtlebot3_world의 pcd파일 예시는 다음과 같다.
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 17139
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 17139
DATA ascii
-4.8000002 4.5 0.029999999
-4.8000002 4.4010987 0.029999999
-4.8000002 4.3021979 0.029999999
-4.8000002 4.2032967 0.029999999
SIZE 4는 4 byte, TYPE F는 float, COUNT 1은 각 필드에서 사용되는 데이터의 개수를 정의합니다. 여기서는 'x', 'y', 'z' 모두 1로, 각 필드에 하나의 값을 가진다는 것을 의미합니다. WIDTH, HEIGHT: 포인트 클라우드의 차원을 정의합니다. 여기서는 WIDTH가 17139로, 포인트의 개수를 나타냅니다. HEIGHT가 1이므로 이는 1D 포인트 클라우드를 나타냅니다.
3. yaml 파일을 다음과 같은 내용으로 만들어 있다.
free_thresh: 0.90
negate: 0
origin: [-4.800000, -4.500000, 0.0]
resolution: 0.100000
mode: trinary
occupied_thresh: 0.95
image: plugin4_map.pgmfree_thresh (자유 임계값): 로봇 맵에서 자유 영역으로 간주되는 확률 값의 임계값입니다. 여기서는 0.90으로 설정되어 이 값보다 작은 확률 값은 자유 영역으로 간주됩니다.
negate (부정): 지도를 부정적으로 표현하는지 여부를 나타내는 플래그입니다. 0은 부정적으로 표현하지 않음을 나타냅니다.
origin (원점): 맵의 원점을 나타내는 세 개의 값으로 구성된 배열입니다. 각 값은 x, y 및 z 좌표를 나타냅니다. 여기서는 [-4.800000, -4.500000, 0.0]으로 설정되어 있습니다.
resolution (해상도): 맵의 해상도를 나타내는 값으로, 미터 단위입니다. 여기서는 0.100000으로 설정되어 있습니다.
occupied_thresh (점유 임계값): 로봇 맵에서 점유 영역으로 간주되는 확률 값의 임계값입니다. 여기서는 0.95로 설정되어 있습니다. 이 값보다 큰 확률 값은 점유 영역으로 간주됩니다.
mode (모드): 맵의 표현 모드를 나타냅니다. 여기서는 'trinary'로 설정되어 있습니다. 'trinary' 모드는 확률 값이 자유, 점유 또는 미지의 세 가지 값 중 하나로 표현됨을 의미합니다. 즉 0.9보다 작으면 자유영역, 09 ~ 0.95 까지는 점유지역, 0.95~ 보다 크면 미지의 영역이다.
image (이미지): 맵 데이터가 저장된 이미지 파일의 경로를 나타냅니다. 여기서는 "plugin4_map.pgm"으로 설정되어 있습니다. 이는 Portable Graymap Format(PGM)의 이미지 파일이다.
4. turtlebot3-world에서 만들은 pcd 파일의 x,y,z의 min~max 값을 알아내면 다음과 같았다.
X: (-4.8000002, 4.7010317)
Y: (-4.4010992, 4.5)
Z: (0.029999999, 1.524375)
5. pcd 파일을 파이썬에서 visualize하면 다음과 같다.
import open3d as o3d
def visualize_pcd(pcd_file_path):
pcd = o3d.io.read_point_cloud(pcd_file_path)
o3d.visualization.draw_geometries([pcd])
if __name__ == '__main__':
pcd_file_path = 'plugin5_map.pcd'
visualize_pcd(pcd_file_path)
6. 해당 pcd 파일에서 Z 값을 변경하면서 새로운 pgm 파일을 만들어 네비게이션에 활용해 보기로 한다.
다음과 같은 파이썬 프로그램을 작성하였다.
import numpy as np
from PIL import Image #pip install Pillow
import yaml # pip install pyyaml
def save_pgm_image(array, filename):
# 배열의 최소 및 최대 값을 정규화하여 0~255 범위로 매핑
normalized_array = ((array - np.min(array)) / (np.max(array) - np.min(array))) * 255
# 소수점 이하를 정수로 변환
normalized_array = normalized_array.astype(np.uint8)
# PGM 이미지 파일 헤더 작성
height, width = normalized_array.shape
header = f"P2\n{width} {height}\n255\n"
# PGM 이미지 파일 작성
with open(filename, 'w') as file:
file.write(header)
np.savetxt(file, normalized_array, fmt='%d', delimiter=' ')
def get_min_max_coordinates_from_pcd(pcd_file_path,altitude, resolution):
array_2d = [[255 for _ in range(256)] for _ in range(256)]
with open(pcd_file_path, 'r') as file:
lines = file.readlines()
# 데이터 시작 라인 찾기
data_start = lines.index("DATA ascii\n") + 1 if "DATA ascii\n" in lines else None
if data_start is None:
raise ValueError("Invalid PCD file format. DATA ascii not found.")
# 데이터 부분만 추출
data_lines = lines[data_start:]
# x, y, z 좌표 추출 및 최소값 및 최대값 계산
x_values = []
y_values = []
z_values = []
for line in data_lines:
values = [float(v) for v in line.split()]
x_values.append(values[0])
y_values.append(values[1])
z_values.append(values[2])
j= values[0]/resolution+256/2
i= -values[1]/resolution+256/2
if(abs(values[2] - altitude)<0.1):
if 0 <= i < 256 and 0 <= j < 256:
array_2d[int(i)][int(j)]=0
min_x, max_x = min(x_values), max(x_values)
min_y, max_y = min(y_values), max(y_values)
min_z, max_z = min(z_values), max(z_values)
return min_x, max_x, min_y, max_y, min_z, max_z,array_2d
if __name__ == '__main__':
image_size = 256
resolution = 0.05
altitude = 0.8 # Z is altitude
pcd_file_path = 'plugin5_map.pcd' #<== input pcd file name
output_file_name='myout' #<== output file name
pgm_file_name=output_file_name+'.pgm'
png_file_name = output_file_name+'.png'
yaml_file_name = output_file_name+'.yaml'
# 최소 최대값 출력
min_x,max_x, min_y,max_y, min_z,max_z,array_2d = get_min_max_coordinates_from_pcd(pcd_file_path,altitude,resolution)
x_range= max_x - min_x
y_range= max_y - min_y
z_range= max_z - min_z
print(min_x,max_x, min_y,max_y, min_z,max_z)
print(f"Image saved to {pgm_file_name}")
save_pgm_image(array_2d, pgm_file_name)
print(f"Image saved to {png_file_name}")
data_array = np.array(array_2d, dtype=np.uint8)
image = Image.fromarray(data_array)
image.save(png_file_name)
print(f"YAML file '{yaml_file_name}' has been created.")
data = {
'mode': 'trinary',
'occupied_thresh': 0.95,
'free_thresh': 0.90,
'negate': 0,
'origin': [-image_size*resolution/2, -image_size*resolution/2, 0.0],
'resolution': resolution,
'image': pgm_file_name
}
with open(yaml_file_name, 'w') as file:
yaml.dump(data, file, default_flow_style=False)
네비게이션에 적용 테스트 하였다. 기존 SALM으로 작성한 이미지와 같이 잘 작동한다.