TOF 센서 데이터를 ROS로 Publish

프로젝트 개요

이 문서는 ROS를 활용하는 다른 노드들과 같이 쓰기 위해 TOF 센서의 raw 데이터를 변환하여 ROS topic으로 발행하는 법을 기술한 문서이다.

ROS로 publish

우선 다음 두 repo를 홈 디렉토리(/home/ubuntu)에 클론해온다.

하나의 repo는 회사 repo이므로 비공개 처리했다.

 

https://github.com/gksma/tof_vis

git clone https://github.com/gksma/tof_vis.git

그 후에 /회사 깃허브/vl53l8cx_python 폴더에 들어가서 VL53L8CX driver를 설치해준다.

 

cd <회사 깃허브>/vl53l8cx_python/
sudo python3 setup.py install

설치가 됐다면 이번엔 tof_vis로 들어가 colcon build를 해주고 source 설정도 해준다.

cd /tof_vis
colcon build
source install/setup.bash

이제 다음 명령으로 tof 데이터를 ros로 publish하는 노드를 실행한다.

ros2 run vl53l8cx_publisher vl53l8cx_node

정상적으로 실행이 됐다면 다음과 같이 메세지가 뜰 것이다.

init 1
init 2
resolution:64
frequency:15hz
[INFO] [1740030238.401766072] [vl53l8cx_publisher]: VL53L8CX PointCloud2 Publisher Started!

이제 도커로 가서 rviz로 시각화를 해보면 다음과 같이 결과가 잘 나오는것을 볼 수 있다.

장애물과의 거리에 따라 z축의 값이 달라진다.

Troubleshooting

1. publish node 종료되는 이슈

해결 : if not distance_values 일때 원래는 바로 sensor를 재시작 하는 시도를 했는데 fail_count 변수를 추가하고 fail_count == 3일때까지 대기하게 짜줬다.

위 사진과 같이 fail_count가 1이 될때쯤에 데이터를 정상적으로 받아오게되면, fail_count = 0으로 초기화하고 다시 데이터를 받는것을 볼 수 있다.

2. CPU 사용량 점차 증가

첫번째 사진이 publish node를 켠 직후고 두번째 사진이 켜고 40분이 지났을 때다. CPU 사용량이 점점 늘어나는것을 볼 수 있다.

1차 해결법 :

    def read_tof_data(self):
        """TOF 센서에서 실제 거리 데이터를 읽고 XYZ 좌표로 변환"""
        try:
            distance_values = self.sensor.get_data()  # 기존 visualize 코드 방식 적용
            if not distance_values:
                self.get_logger().error("Failed to get TOF data, reinitializing sensor...")
                self.sensor._initialize_driver()  # 센서 다시 초기화
                return None

            # 거리 데이터를 저장할 배열 초기화
            distance_value = np.full((64,), 4000, dtype=np.float32)  # 기본값 4000mm (센서 읽기 오류 대비)
            
            for i in range(len(distance_values)):
                zone = distance_values[i]['zone']
                status = distance_values[i]['Status']
                if status == 5:  # 유효한 거리 데이터만 반영
                    distance_value[zone] = distance_values[i]['Distance(mm)']

            # 3D 좌표 변환 (z는 실제 거리값)
            buf = np.empty((self.res, self.res, 3), dtype=np.float32)
            flipped_data = np.fliplr(distance_value.reshape(8, 8))  # `visualize.py`와 동일하게 좌우 반전

            for i in range(8):
                for j in range(8):
                    x = (i - 3.5) * 0.05  # 가로 위치 보정
                    y = (j - 3.5) * 0.05  # 세로 위치 보정
                    z = flipped_data[i, j] / 1000.0  # mm -> m 변환
                    buf[i, j] = [x, y, z]

            return buf
        except Exception as e:
            self.get_logger().error(f"Error reading TOF data: {e}")
            return None

read_tof_data 함수에서 buf는 새로운 배열을 생성하기만 할 뿐 데이터를 해제하진 않는다. 기존에 할당된 데이터를 해제해주는 코드를 추가해준다.

    def read_tof_data(self):
        """TOF 센서에서 실제 거리 데이터를 읽고 XYZ 좌표로 변환"""
        try:
            distance_values = self.sensor.get_data()  # 기존 visualize 코드 방식 적용
            if not distance_values: # 데이터가 안들어왔을때 3번 대기
                self.fail_count += 1
                self.get_logger().warn(f"TOF data not ready(attempt {self.fail_count}/3)")
                if self.fail_count >= 3:
                    self.get_logger().error("Failed to get TOF data 3 times, reinitializing sensor...")
                    self.sensor._initialize_driver()
                    time.sleep(0.5)  # 안정화를 위해 0.5초 대기
                    self.fail_count = 0
                return None

            self.fail_count = 0
            # 거리 데이터를 저장할 배열 초기화
            distance_value = np.full((64,), 4000, dtype=np.float32)  # 기본값 4000mm (센서 읽기 오류 대비)
            
            for i in range(len(distance_values)):
                zone = distance_values[i]['zone']
                status = distance_values[i]['Status']
                if status == 5:  # 유효한 거리 데이터만 반영
                    distance_value[zone] = distance_values[i]['Distance(mm)']

            # 기존 buf 초기화(메모리 누수 방지)
            if hasattr(self, "last_buf") and self.last_buf is not None: # 기존 배열이 있을때만
                del self.last_buf
                self.last_buf = None

            # 3D 좌표 변환 (z는 실제 거리값)
            buf = np.empty((8, 8, 3), dtype=np.float32)
            flipped_data = np.fliplr(distance_value.reshape(8, 8))  # `visualize.py`와 동일하게 좌우 반전

            for i in range(8):
                for j in range(8):
                    x = (i - 3.5) * 0.05  # 가로 위치 보정
                    y = (j - 3.5) * 0.05  # 세로 위치 보정
                    z = flipped_data[i, j] / 1000.0  # mm -> m 변환
                    buf[i, j] = [x, y, z]

            self.last_buf = buf # last_buf로 저장
            return buf
        except Exception as e:
            self.get_logger().error(f"Error reading TOF data: {e}")
            return None

buf로 배열을 생성하고, 함수의 끝에 last_buf로 저장한다.

 

            self.last_buf = buf # last_buf로 저장
            return buf

그리고 buf를 반환하고 다시 tof에서 받아온 데이터를 buf를 생성하여 넣기전에 기존에 있던 last_buf가 있다면 배열을 삭제하고 메모리 누수를 막는다.

            if hasattr(self, "last_buf") and self.last_buf is not None: # 기존 배열이 있을때만
                del self.last_buf
                self.last_buf = None

하지만 여전히 cpu 사용량이 증가하는 문제는 해결하지 못하였다.

 

2차 해결법을 생각하려 했지만 인턴이 내일부로 끝나서 문서 정리중이다.. 해결하고 가지 못한게 아쉽다