반응형
1. 2D/3D LiDAR 센서 연동 및 ROS2 PointCloud2 변환 배경
임베디드 로보틱스 및 피지컬 AI 시스템에서 LiDAR 센서는 주변 환경의 3차원 공간 정보를 수집하는 핵심 하드웨어입니다. 센서로부터 수집되는 원시 데이터(Raw Data)는 UART 시리얼 통신이나 Ethernet UDP/TCP 패킷 형태로 전달됩니다.
이러한 raw 바이너리 패킷을 제어 알고리즘이나 내비게이션 스택(Nav2)에서 활용하기 위해서는 ROS2 표준 메시지 포맷인 sensor_msgs/msg/PointCloud2로 변환해야 합니다. 이 글에서는 2D/3D LiDAR의 인터페이스 통신 데이터를 파싱하고, 메모리 정렬을 고려하여 PointCloud2 메시지로 변환한 뒤 RViz2에서 시각화하는 전체 프로세스를 설명합니다.
2. ROS2 PointCloud2 변환 핵심 요약
- Data Parsing (데이터 파싱): 라이다 메모리 맵 명세(LiDAR Memory Map Specification)에 따라 C++ reinterpret_cast 또는 memcpy를 사용하여 Raw 바이너리 스트림(UART/Ethernet UDP)을 정밀하게 파싱합니다.
- PointCloud2 Conversion (메시지 변환): sensor_msgs::PointCloud2Modifier로 벡터 메모리를 할당하고, sensor_msgs::PointCloud2Iterator를 활용해 x, y, z, intensity 필드 데이터를 효율적으로 매핑합니다.
- RViz2 Visualization (3D 시각화): Header의 frame_id를 laser_frame으로 지정한 후 rclcpp::Publisher<sensor_msgs::msg::PointCloud2>를 통해 퍼블리시하여 RViz2에서 3D 점군 데이터를 시각화합니다.
3. LiDAR 데이터 통신 파싱 및 ROS2 PointCloud2 변환 구현
통신 인터페이스 및 프로토콜 구조 비교
2D LiDAR와 3D LiDAR는 물리 인터페이스와 데이터 전송 방식에서 차이가 있습니다.
| 항목 | 2D LiDAR | 3D LiDAR |
| 인터페이스 | UART / RS-232 / RS-485 | Ethernet (UDP/IP) |
| 데이터 전송 속도 | 115200 ~ 921600 bps | 100 Mbps ~ 1 Gbps |
| 패킷 데이터 구조 | [Header][Angle][Distance][CRC] | [Header][Block Data (Azimuth/Distance/Reflectivity)][Tail] |
| 주요 ROS2 메시지 | sensor_msgs/msg/LaserScan / PointCloud2 | sensor_msgs/msg/PointCloud2 |
Raw 패킷 좌표계 변환 방식
LiDAR 센서가 측정하는 거리는 극좌표계(Polar Coordinates) 형태인 거리($r$), 방위각($\theta$), 고저각($\phi$) 데이터입니다. 이를 3차원 직교좌표계(Cartesian Coordinates) $x, y, z$로 변환하는 공식은 다음과 같습니다.
$$x = r \cdot \cos(\phi) \cdot \cos(\theta)$$
$$y = r \cdot \cos(\phi) \cdot \sin(\theta)$$
$$z = r \cdot \sin(\phi)$$
ROS2 PointCloud2 변환 퍼블리셔 코드 구현
아래 코드는 C++ 기반 ROS2 노드로, 퍼스널 패킷 데이터를 sensor_msgs::msg::PointCloud2 포맷으로 구성하여 퍼블리시하는 예제입니다.
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <cmath>
class LidarPointCloudPublisher : public rclcpp::Node {
public:
LidarPointCloudPublisher() : Node("lidar_pointcloud_publisher") {
publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("pointcloud2_out", 10);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&LidarPointCloudPublisher::publish_cloud, this)
);
}
private:
void publish_cloud() {
auto cloud_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
// Set Header
cloud_msg->header.stamp = this->now();
cloud_msg->header.frame_id = "laser_frame";
// Setup PointCloud2 modifier
sensor_msgs::PointCloud2Modifier modifier(*cloud_msg);
modifier.setPointCloud2FieldsByString(2, "xyz", "intensity");
size_t num_points = 360; // Example point count
modifier.resize(num_points);
// Setup Iterators
sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
sensor_msgs::PointCloud2Iterator<float> iter_intensity(*cloud_msg, "intensity");
// Dummy raw data conversion loop
for (size_t i = 0; i < num_points; ++i) {
float angle_rad = (i * M_PI) / 180.0f;
float distance = 2.0f; // 2 meters test distance
// Convert Polar to Cartesian coordinates
*iter_x = distance * std::cos(angle_rad);
*iter_y = distance * std::sin(angle_rad);
*iter_z = 0.0f;
*iter_intensity = 255.0f;
++iter_x;
++iter_y;
++iter_z;
++iter_intensity;
}
publisher_->publish(*cloud_msg);
}
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<LidarPointCloudPublisher>());
rclcpp::shutdown();
return 0;
}
4. 실무 개발 및 디버깅 팁
- Socket & Serial Buffer Optimization: Ethernet UDP 패킷 수신 시 소켓 버퍼 크기가 작으면 패킷 유실(Packet Loss)이 발생합니다. setsockopt를 사용하여 SO_RCVBUF 크기를 늘려야 합니다.
- ROS2 Topic Echo Validation: 데이터가 올바르게 퍼블리시되는지 확인하려면 터미널에서 다음 명령어를 실행합니다.
ros2 topic echo /pointcloud2_out --no-arr - RFT (Real-Time Fine Tuning): CPU 사용량을 줄이려면 sensor_msgs::PointCloud2Iterator 사용 시 루프 내부에서 과도한 메모리 할당이 일어나지 않도록 예외 처리를 해야 합니다.
5. 엔지니어링 과정에서 흔히 하는 실수 및 해결법
1. Frame ID 미설정으로 인한 RViz2 표시 오류
- 증상: RViz2에서 Global Status: Error 메시지가 출력되고 점군이 표시되지 않음.
- 원인: cloud_msg->header.frame_id 설정이 빠졌거나 RViz2의 Fixed Frame 이름과 일치하지 않음.
- 해결법: frame_id를 "laser_frame" 또는 "base_link"로 지정하고, RViz2의 Global Options -> Fixed Frame 값을 동일하게 설정합니다.
2. Endianness 및 Structure Padding 오류
- 증상: C/C++ struct로 패킷을 직접 캐스팅할 때 $x, y, z$ 좌표 값이 비정상적으로 계산됨.
- 원인: 컴파일러 기본 메모리 얼라인먼트로 인해 구조체 내부에 패딩 바이트가 삽입됨.
- 해결법: 패킷 정의 구조체 선언 시 #pragma pack(push, 1) 구문을 사용하여 바이트 정렬을 강제합니다.
#pragma pack(push, 1)
struct LidarPacketHeader {
uint16_t sync_bytes;
uint8_t packet_type;
uint32_t timestamp;
};
#pragma pack(pop)
6. 결론
LiDAR 센서 인터페이스 통신 데이터를 파싱하고 이를 ROS2 PointCloud2 메시지로 변환하는 프로세스는 로봇 퍼셉션(Perception)의 기초 단계입니다. 본 가이드에서 다룬 데이터 파싱 방식, 극좌표-직교좌표 변환, 메모리 정렬 문제 해결법 및 PointCloud2Iterator 패턴을 적용하면 안정적인 점군 데이터 처리 노드를 구현할 수 있습니다.
반응형