최근 Velodyne 라이다 VLP-16을 사용해 볼 기회가 생겼다. 이 라이다는 자율주행 자동차에 많이 사용되고 있지만 나는 라이다를 고정하고 특정 목표물의 시간에 따른 위치변화(변이)를 확인하고자 테스트를 시작했고 결과물로 파이썬에서 라이다 데이터를 실시간으로 읽어서 파일 혹은 데이터 베이스에 저장할 목적으로 테스트를 했다.
ROS
라이다를 구입할 때 받은 VeloView라는 프로그램으로 라이다가 정상적으로 작동하고 PCAP 또는 CSV 형태의 파일로 저장 가능한 것을 확인했지만 그건 내가 원하는 방법이 아니었다.
며칠의 삽질끝에 ROS를 이용하는 것이 가장(?) 효율적이라는 결론을 얻고 Ubuntu 20.04에 ROS – Noetic을 설치했다. 아래의 링크에서 ROS 설치 방법을 확인할 수 있다.
ROS - Noetic 설치 http://wiki.ros.org/noetic/Installation/Ubuntu
Velodyne Driver 설치
ROS를 이용하는 가장 큰 이유는 이미 만들어진 드라이버를 활용할 수 있기 때문인데 Velodyne Lidar 역시 버전별로 드라이버가 있다. 아래의 링크와 같이 드라이버를 설치했다. 링크에는 최초에 라이다를 연결하는 방법부터 rviz를 이용하여 VeloView와 같은 형식으로 라이다 데이터를 조회 할 수 있는 방법까지 알려준다.
아래 링크의 내용에서 sudo apt-get install ros-VERSION-velodyne 부분은 sudo apt-get install ros-noetic-velodyne 으로 변경해서 설치해 준다 Velodyne Driver 설치 http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16
ROS – RViz 에서 데이터를 확인할 수 있었지만 위치 데이터를 얻는 것이 목적이었기 때문에 VeloView와 동일하게 ROS에서 잘 연결되는구나를 확인하고 다음 삽질을 시작했다. 목표는 파이썬에서 각도별 거리를 얻어내는 것.
Publisher, Subscriber, PointCloud, Python
파이썬으로 라이다 데이터를 추출하기 위하여 ROS 튜토리얼을 스터디하던 과정에서 ROS 노드들의 통신 방식에 대해서 알게 되었다. 전에 내가 IOT 장비 연결에 주로 사용하던 MQTT 프로토콜과 거의 흡사한 구조.
발행자(Publisher)와 수신자(Subscriber)가 일정한 토픽(topic)을 공유하고 데이터를 송, 수신하는 방식으로 아래 링크를 참조하면 파이썬으로 데이터를 송, 수신하는 샘플을 확인할 수 있다.
위의 RViz 프로그램 역시 토픽을 Subscribe하여 라이다의 3차원 이미지를 그린다.
파이썬을 이용한 ROS 데이터 송수신 샘플 http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29
아래의 커맨드를 실행해서 퍼블리셔(Publisher)를 실행시킨다. Velodyne Driver 설치 링크에서 이미 Publisher를 실행했었다. 그래서 RViz가 라이다 데이터를 수신해서 결과 화면을 그릴 수 있었다. 종료되었다면 아래와 같이 다시 실행할 수 있다.
$ roslaunch velodyne_pointcloud VLP16_points.launch
위의 샘플 코드를 응용하여 아래와 같이 벨로다인 토픽을 수신(Subscribe)한다.
if __name__ == '__main__':
rospy.init_node("listener", anonymous=True)
rospy.Subscriber('/velodyne_points', PointCloud2, callback)
rospy.spin()
위의 코드에서 rospy.Subscriber() 의 파라메타는 토픽, 수신할 구조체 형식, 콜백 함수인데 콜백 함수의 구조는 아래와 같다.
import sensor_msgs.point_cloud2 as pc2
def callback(input_ros_msg):
for data in pc2.read_points(input_ros_msg, skip_nans=True):
x2 = data[0]
y2 = data[1]
z2 = data[2]
intensity = data[3]
ring = data[4]
time1 = datetime.fromtimestamp(data[5])
distance = math.sqrt((x2)**2+(y2)**2+(z2)**2)
azimuth = math.degrees(math.atan2((x2),(y2)))
콜백을 통하여 전달된 데이터는 PointCloud 구조로 변환하고 변환된 좌표를 이용하여 거리와 방위각을 계산할 수 있다. 이제 데이터를 얻었으니 저장만하면 된다 . ^_^
거리와 방위각 계산방법은 아래 링크를 참고했다. https://gis.stackexchange.com/questions/108547/how-to-calculate-distance-azimuth-and-dip-from-two-xyz-coordinates
2022.07.07 박병일