영칠이의 개발로그

MQTT 통신

yeong77 — Mon, 1 Sep 2025 18:14:16 +0900

1. MQTT란?

MQTT는 가볍고 효율적인 Pub/Sub 메시징 프로토콜로, 저전력·저대역폭 환경에서 센서 데이터나 제어 명령을 주고받는 데 많이 쓰이며, 드론·IoT·모바일 기기 통신에 적합합니다.
구조:

Broker: 메시지 중계 서버 (예: Mosquitto)

Publisher: 메시지를 보내는 쪽

Subscriber: 메시지를 받는 쪽

2. 로컬에서 MQTT Broker 설치

가장 많이 쓰이는 건 Eclipse Mosquitto입니다.

Ubuntu / Debian 계열

sudo apt install mosquitto mosquitto-clients -y

3. Mosquitto 실행하기

Ubuntu에서 기본 실행

sudo systemctl enable mosquitto  
sudo systemctl start mosquitto

4. 메시지 주고받기 (로컬 테스트)

구독하기 (Subscriber)

터미널 1:

mosquitto_sub -h localhost -t "test/topic"

-h localhost: 브로커 주소 (로컬이므로 localhost)
-t "test/topic": 구독할 토픽 이름

발행하기 (Publisher)

터미널 2:

mosquitto_pub -h localhost -t "test/topic" -m "Hello MQTT!"

그럼 터미널 1에서 메시지가 출력됨:

Hello MQTT!

LIO-SAM 코드 리뷰

yeong77 — Fri, 29 Aug 2025 16:37:52 +0900

1. LIO-SAM 핵심 개념

-Tightly-coupled fusion
→ LiDAR와 IMU 데이터를 하나의 최적화 문제로 강하게 결합
- Factor-graph based estimation
→ 요인 그래프(Factor Graph)로 포즈와 상태를 동시에 추정
- Old scan marginalization → Pose optimization
→ 오래된 스캔은 제거(마진화)하고, 최신 스캔 중심으로 자세 최적화
- Local scan matching for real-time
→ 국소적인 스캔 매칭만 수행하여 실시간 처리 가능
- Sliding window
→ 일정 구간(윈도우) 안의 데이터만 유지하며 연속적으로 최적화

2. FLOW

- 전체 메인 코드는 이렇게 이루어지고 그 중 두 프로세스(Feature Extraction, Image Projection)는 라이다 데이터를 전처리 하는데 사용됩니다.

3. Main Code

3.1 IMU Preintegration

3.2 Image Projection / Feature Extraction

라이다는 한 프레임 안에서도 포인트마다 측정 시간이 달라 왜곡이 생기는데요.
이를 IMU 데이터를 활용한 deskewing으로 보정해 모든 포인트를 같은 시간 기준으로 맞춥니다.
그 후 Feature Extraction 단계에서 코너(edge)와 평면(surface) 포인트를 뽑아내어 SLAM에 활용합니다.

3.3 Map Optimization

1) loop-closure factor

2) odom factor

3) gps factor

4) 최적화

LIO-SAM 논문 리뷰

yeong77 — Tue, 26 Aug 2025 18:00:38 +0900

LIO-SAM: Factor Graph 기반 LiDAR-IMU 융합 프레임워크 정리

최근 로봇의 자율주행 및 정밀한 위치 추정을 위해 다중 센서 융합이 활발히 연구되고 있습니다. 그중에서도 LiDAR-IMU 융합은 조명 변화나 시각적 특성에 크게 의존하지 않고, 안정적인 3D 환경 이해와 궤적 추정을 가능하게 해 주어 많은 주목을 받고 있습니다. 이번 글에서는 대표적인 LiDAR-IMU 융합 프레임워크인 LIO-SAM (LiDAR-Inertial Odometry via Smoothing and Mapping) 논문을 정리해 보았습니다.

1. 연구 배경

비전 기반 방법: 장소 인식에는 강점이 있지만 조명 변화와 관측 거리 변화에 취약.
LiDAR 기반 방법: 환경의 기하학적 구조를 안정적으로 반영 가능.
기존 연구의 한계
- LOAM: 낮은 드리프트와 루프 클로저 탐지가 가능하지만,
  - 전역 맵 기반이라 루프 클로저 발생 시 이전 위치를 수정하기 어려움
  - GPS 통합에도 제약
  - 매번 스캔 매칭 수행 → 누적 드리프트 발생
- LeGO-LOAM: UGV 환경에 맞게 단순화, 하지만 IMU 최적화를 고려하지 않음.
- LIOM: tightly-coupled 구조지만 모든 센서 데이터를 처리해야 해서 실시간성이 떨어짐.

2. LIO-SAM의 핵심 아이디어

LIO-SAM은 tightly-coupled 방식으로 LiDAR와 IMU를 팩터 그래프 내에서 공동 최적화합니다.

IMU Pre-integration: LiDAR 스캔 중 발생하는 움직임을 추정하여 포인트 클라우드 왜곡 제거 및 초기 추정값 제공.
LiDAR Odometry 결과: IMU 바이어스 추정에 활용.
Factor Graph 최적화: IMU, LiDAR, GPS, 루프 클로저 팩터를 동시에 고려.
실시간성 확보: 오래된 스캔은 마진화(marginalization)하고, 선택적 keyframe + sliding window 방식으로 계산량을 제어.

3. 시스템 개요

LIO-SAM의 상태 벡터는

으로, **자세(R), 위치(p), 속도(v), IMU 바이어스(b)**를 포함합니다.

최적화는 MAP(Maximum a Posteriori) 추정으로 정식화되며, 노이즈를 가우시안으로 가정하여 비선형 최소제곱 문제로 변환됩니다.

팩터 그래프 구성 요소

IMU 사전적분 팩터
LiDAR 오도메트리 팩터
GPS 팩터
루프 클로저 팩터

최적화는 iSAM2 기반 점진적 스무딩으로 실시간에 가깝게 수행됩니다.

4. 주요 구성 요소

A. IMU 사전적분 (Pre-integration Factor)

가속도·각속도 데이터를 적분해 상대적인 바디 움직임 추정.
사전적분 방식을 사용하여 계산 효율성 확보.
LiDAR Odometry 팩터와 함께 IMU 바이어스를 공동 최적화.

B. LiDAR Odometry Factor

특징 추출:
- roughness 큰 점 → edge 특징
- roughness 작은 점 → plane 특징
Keyframe 선택:
- 위치 변화 1m 이상 or 회전 변화 10° 이상일 때 등록.
- 최근 n개의 keyframe만 유지 (sliding window).
Scan Matching:
- 새 프레임을 기존 sub-keyframe map에 정합.
- 초기 추정은 IMU 예측값 사용.
- 최적화로 두 포즈 간 상대 변환 ΔT 추정 → LiDAR odometry factor로 연결.

C. GPS Factor

장시간 내비게이션에서 드리프트를 줄이기 위해 절대 위치 센서 사용.
GPS 데이터를 로컬 좌표계로 변환하여 factor graph에 추가.
공분산 비교를 통해 신뢰성 있는 경우에만 GPS 팩터 반영.

D. Loop Closure Factor

새 keyframe이 추가될 때 과거 keyframe과의 유클리드 거리 기반으로 후보 탐색.
Scan matching 후 상대 변환을 loop closure factor로 추가.
시간적으로 멀리 떨어져 있지만 공간적으로 가까운 경우 효과적.

5. LIO-SAM의 특징 및 기여

Tightly-coupled LIDAR-IMU fusion: IMU 바이어스까지 고려한 정밀한 최적화.
Factor Graph 기반 전역 최적화: LiDAR, IMU, GPS, 루프 클로저 동시 고려.
Sliding Window: 실시간성을 보장하며 오래된 스캔은 마진화.
다양한 환경에서 검증: 실내/실외, 대규모 공간에서 모두 효과적.

JT16 LiDAR 사용법

yeong77 — Fri, 8 Aug 2025 11:24:43 +0900

JT라이다 16채널 사용법을 포스팅 하겠습니다.

JT라이다는 아래의 ROS 패키지와 SDK를 이용하였습니다.

JT 라이다 ROS 패키지

JT 라이다 SDK

JT LiDAR 사용법

레포지토리 복사

git clone https://github.com/HesaiTechnology/HesaiLidar_ROS_2.0.git

cd HesaiLidar_ROS_2.0/src/driver
git clone https://github.com/HesaiTechnology/HesaiLidar_SDK_2.0.git

관련 라이브러리 설치

sudo apt update && sudo apt install -y libpcl-dev libpcap-dev libyaml-cpp-dev openssl

SDK 빌드

cd HesaiLidar_SDK_2.0
mkdir -p build && cd build
cmake -DCMAKE_BUILD_TYPE=Release ..
make -j$(nproc)

워크스페이스 빌드

colcon build

패키지 빌드를 다하면 이제 라이다에 있는 rs485 케이블과 rs232 케이블 각각을 pc에 연결 후 포트이름을 체크하고 권한을 줍니다!
둘 다 ttyUSB*일수도 있고 ttyACM*일수도 있어서 체크해 봐야 합니다.

포트 확인

ls -l /dev/ttyUSB*
sudo chmod 666 /dev/ttyUSB*

포트 번호를 확인 후 아래와 같이 config.yaml 파일에서 serial로 연결할 경우 source_type을 4로 하고, 포트 번호를 저장해줍니다.
그리고 라이다 센서의 보정 데이터가 있는 csv파일의 경로도 넣어줍니다.

파라미터 수정

source_type : 4 
serial_type:
	rs485_com: "/dev/ttyUSB0"
	RS232_COM: "/dev/ttyACM0"
	point_cloud_baudrate: 3125000
	correction_save_path:
	correction_file_path: "/home/yy/ros2_ws/src/HesaiLidar_ROS_2.0/Correction file/JT16-JT35CA5D9235C956-Angle-Fix.csv"

실행

ros2 launch hesai_ros_driver start.py

결과사진

Docker GPU 연결

yeong77 — Thu, 7 Aug 2025 11:22:40 +0900

이번엔 docker에서 gpu 사용가능하게 하는 방법을 포스팅하겠습니다.

저의 경우에는 GPU가 오픈 소스 커널 모듈(nvidia-open)만 지원이 가능해서 거기에 맞게 설치하였습니다.

1️⃣ 기존 NVIDIA 드라이버 완전 제거

sudo apt purge '^nvidia'
sudo apt autoremove --purge
sudo rm -rf /etc/X11/xorg.conf
sudo update-initramfs -u

2️⃣ 최신 NVIDIA 드라이버 (Open 모드 포함) 재설치

Ubudo ubuntu-drivers devices
sudo apt install nvidia-driver-550-open
sudo reboot
nvidia-smi

3️⃣ NVIDIA Container Toolkit 설치 여부 확인

dpkg -l | grep nvidia-container

없을경우 설치

distribution=$(. /etc/os-release;echo $ID$VERSION_ID) && \
sudo mkdir -p /etc/systemd/system/docker.service.d && \
curl -s -L https://nvidia.github.io/libnvidia-container/gpgkey | sudo apt-key add - && \
curl -s -L https://nvidia.github.io/libnvidia-container/$distribution/libnvidia-container.list | \
  sudo tee /etc/apt/sources.list.d/nvidia-container.list

sudo apt update
sudo apt install -y nvidia-docker2
sudo systemctl restart docke```

Gazebo Sensor Plugin

yeong77 — Wed, 6 Aug 2025 14:13:06 +0900

오늘은 저번에 이어서 가제보 환경에 라이다와 IMU 센서를 플러그인 해서 ROS 데이터를 받아오는 것을 포스팅하겠습니다 !

1. LiDAR Plugin

제가 여기서 사용한 라이다 모델은 Velodyne이고, Gazebo-classic 환경입니다.

1.1 git clone

먼저 velodyne 모델이 있는 git repository를 clone 한 후 아래의 경로에 붙여넣어 줍니다.

경로

PX4-Autopilot/Tools/simulation/gazebo-classic/models

git clone

git clone https://github.com/osrf/gazebo_models.git

1.2 model joint 연결

이후 아래 경로의 model.sdf 에서 다음과 같이 라이다를 include 해주고 joint를 넣어줘서 기존의 드론 모델과 결합을 시켜줍니다.

경로

PX4-Autopilot/Tools/simulation/gazebo-classic/models/iris/model.sdf

코드

<include name="velodyne">
      <uri>model://velodyne_hdl32</uri>
      <pose>0 0 -0.1 0 0 0</pose> <!-- 드론 중심 기준 위치 -->
    </include>

    <joint name="velodyne_joint" type="fixed">
      <parent>base_link</parent>  <!-- 드론의 메인 링크 -->
      <child>velodyne::base</child> <!-- velodyne 모델의 링크 이름 -->
    </joint>

1.3 sensor 플러그인

아래 센서를 사용하기 위해서는 일단 아래와 같은 패키지를 설치해야 합니다.

sudo apt install ros-humble-gazebo-ros-pkgs ros-humble-gazebo-ros2-control
sudo apt install ros-humble-velodyne-description ros-humble-velodyne-gazebo-plugins ros-humble-velodyne-simulator

이후 velodyne 모델 sdf 파일에서 센서 블록안에 다음과 같이 추가를 해줍니다. 아래를 추가해줘야 시뮬레이션을 실행했을 때 ROS 메시지가 나옵니다 !

여기서의 토픽이름은 "/velodyne_points" 입니다.

<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
          <ros>
            <namespace>/velodyne</namespace>
            <remapping>~/out:=velodyne_points</remapping>
          </ros>
          <frame_name>top</frame_name>
          <organize_cloud>false</organize_cloud>
          <min_range>0.9</min_range>
          <max_range>130.0</max_range>
          <gaussian_noise>0.008</gaussian_noise>
        </plugin>

1.4 라이다 스캔 조정

1.3 까지 하고 라이다를 스캔을 시각화했을 때 포인트 수가 적고 시야각이 좁아서 다음과 같이 샘플 수와 시야각을 늘려 주었습니다. 다음은 라이다의 모델 sdf 파일에서 ray 블록안에 추가해주면 됩니다.

<scan>
  <horizontal>
    <samples>1024</samples>
    <resolution>1</resolution>
    <min_angle>-3.1415</min_angle>
    <max_angle>3.1415</max_angle>
  </horizontal>
  <vertical>
    <samples>128</samples>
    <resolution>1</resolution>
    <min_angle>-3.1415</min_angle>
    <max_angle>3.1415</max_angle> 
  </vertical>
</scan>

2. IMU Plugin

다음은 IMU Plugin 입니다. 라이다에서는 include 했지만 imu는 바로 모델 sdf 파일에 센서 플러그인을 하였습니다. 크게 다음과 같이 추가했습니다.

mass
pose
plugin
jointmass 같은 경우에는 추가하지 않으면 드론이 제대로 날지 않았어서 추가해줬습니다. 그리고 질량이 너무 크면 드론이 천천히 날고, waypoint를 주었을 때 급하강했다가 상승하는 현상이 보이기 때문에 적당한 값을 넣어줘야 합니다.

<link name="imu_link">
      <pose>0.05 0 -0.1 0 0 0</pose>

      <inertial>
        <mass>0.001</mass>
        <inertia>
          <ixx>1e-6</ixx>
          <iyy>1e-6</iyy>
          <izz>1e-6</izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
      
      <sensor name="imu_sensor" type="imu">
        <pose>0 0 0 0 0 0</pose>  <!-- IMU 위치 설정 -->
        <always_on>true</always_on>
        <update_rate>100.0</update_rate>
        <visualize>true</visualize>
        <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
            <ros>
              <namespace>/imu</namespace>
              <remapping>~/out:=data</remapping>
            </ros>
            <topicName>imu/data</topicName>
            <frameName>imu_link</frameName>
          </plugin>
      </sensor>
    </link>

    <joint name="imu_joint" type="fixed">
      <parent>base_link</parent>
      <child>imu_link</child>
    </joint>

3. 결과

센서 실행은 저번에 포스팅한 드론 실행하는 command를 사용하면 센서가 활성화가 됩니다 !

topic list

라이다 topic echo

imu topic echo

Scan 시각화

감사합니다 .

PX4 simulation

yeong77 — Tue, 5 Aug 2025 14:10:32 +0900

오늘은 px4 설치하고 가제보 환경에서 제어하는 것을 포스팅 하려고 합니다.
이후에는 ROS를 사용해서 드론을 제어하고 라이다와 IMU를 플러그인해서 센서 데이터까지 받아오는 것을 포스트하겠습니다.

px4 설치

git clone https://github.com/PX4/PX4-Autopilot.git --recursive
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
cd /ros2_ws/src/PX4-Autopilot

빌드 및 가제보 실행

cd /ros2_ws/src/PX4-Autopilot
make px4_sitl gazebo-classic
make px4_sitl gazebo-classic_iris__warehouse (가제보 환경 있는 버전)

다음과 같이 시뮬레이션 환경이 열리면 'Ready for takeoff'가 뜨는데, 이게 뜨면 pxh> 에다가 원하는 명령어를 입력하면 됩니다.

제어

commander takeoff
commander land

GCS 설치

처음에는 GCS가 있어야 Takeoff가 가능했는데 지금은 없어도 가능하네요 ..? 아무튼 네

아래 링크에서 다운 받으면 됩니당
Qgroundcontrol

도커를 사용하는 경우에는 root에서 실행을 못하고, user에서 실행할 수 있습니다.

그렇기 때문에 일반 사용자 권한 허용을 해준후 user모드로 들어가서 해야됩니다.

//권한 허용
usermod -a -G dialout $user

//user mode
su - $user

결과

아래 사진과 같이 commander takeoff를 했을 때 드론이 이륙하는것을 볼 수 있습니다.

ROS bridge

yeong77 — Mon, 4 Aug 2025 14:08:50 +0900

오늘은 ROS1과 ROS2 통신이 가능하도록 하는 ROS Bridge에 대해 다뤄보려고 합니다.

참고한 깃허브는 다음과 같습니다.

ros1_bridge

우선 두 환경에서 통신이 되게 하려면 같은 우분투 환경에서 ROS1과 ROS2가 설치가 되어야 합니다.

제가 사용한 개발환경은 Ubuntu 20.04 이고, 같은 환경에 foxy와 noetic이 설치가 되어있습니다.

1️⃣ 설치 방법

1) ros1_bridge 패키지 설치 (ROS2 foxy 기준)

sudo apt update
sudo apt install -y ros-foxy-ros1-bridge

2) ROS1과 ROS2 환경 설정

각각의 터미널에서 해당하는 ROS 버전을 로드.

✔️ ROS1 (예: Noetic)

source /opt/ros/noetic/setup.bash
roscore

✔️ ROS2 (예: Foxy)

source /opt/ros/foxy/setup.bash

3) 브리지 실행 (옵션 추가하여 강제 브리지)

ros2 run ros1_bridge dynamic_bridge --bridge-all-topics

✅ 이제 ROS1과 ROS2 간에 자동으로 브리지가 설정됨!

) ROS2에서 메시지 발행 & ROS1에서 수신

✔️ ROS2에서 메시지 발행

source /opt/ros/foxy/setup.bash
ros2 topic pub /number std_msgs/msg/Int32 "{data: 42}"

✔️ ROS1에서 메시지 수신

source /opt/ros/noetic/setup.bash
rostopic echo /number

✅ ROS2에서 보낸 메시지가 ROS1에서 정상적으로 보이면 성공!

주의사항
일단 저의 경우에는 원래 humble과 noetic에서 통신을 하려고 했는데, 같은 우분투 환경안에서 해야하다 보니 noetic은 20.04이고 humble은 22.04라서 불가능 했습니다. 그리고 같은 우분투 환경이기 때문에 도커 컨테이너에서도 되지 않았네요. humble 버전을 사용하고 싶은데 혹시 아시는 분 있으면 댓글 부탁드립니다

2️⃣ 결과

다음과 같이 오른쪽 foxy에서 메시지를 publish하면 왼쪽 noetic에서 subscribe하는 것을 볼 수 있습니다.

LiDAR와 IMU 센서 동기화

yeong77 — Sun, 3 Aug 2025 14:05:19 +0900

LIO-SAM은 두 센서의 데이터를 융합해 정밀한 위치 추정을 수행하므로, 시간적으로 일치된 데이터가 필수입니다.

LiDAR는 저주파, IMU는 고주파이기 때문에 IMU 데이터를 LiDAR 주기에 맞춰 동기화해야 합니다. 이를 위해 ROS의 메시지 필터를 사용했고, 약간의 시간 오차를 허용하는 ApproximateTime 정책을 적용했습니다.

처음엔 ExactTime으로 시도했지만, 시간이 너무 정확히 일치해야 해서 동기화가 잘 되지 않더라구요.

‍♂️

☑️코드 작성(sync_imu.cpp)

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <rclcpp/qos.hpp>

using std::placeholders::_1;
using std::placeholders::_2;

class SyncLidarImuNode : public rclcpp::Node
{
public:
    SyncLidarImuNode() : Node("sync_lidar_imu")
    {
        RCLCPP_INFO(this->get_logger(), "IMU & LiDAR Synchronization Node Started.");

        // '/ouster/points'의 퍼블리셔가 'BEST_EFFORT'이므로, Subscriber도 'BEST_EFFORT'로 설정
        rclcpp::QoS qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
        qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        qos_profile.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);

        rclcpp::QoS imu_qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
        imu_qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        imu_qos_profile.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);

        // SubscriptionOptions 사용하여 QoS 강제적용
        rclcpp::SubscriptionOptions options;
        options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();

        // message_filters::Subscriber에서도 `BEST_EFFORT` 적용
        imu_sub_.subscribe(this, "/imu/data", rmw_qos_profile_sensor_data);
        lidar_sub_.subscribe(this, "/ouster/points", rmw_qos_profile_sensor_data);

        // Approximate Time Synchronizer 동기화 정책 설정 
        sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(20), imu_sub_, lidar_sub_);
        sync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(0.017)); //best
        sync_->registerCallback(std::bind(&SyncLidarImuNode::callback, this, _1, _2));

        // 동기화된 IMU 메시지 publish
        imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/synced_imu", imu_qos_profile);
        lidar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/synced_points", qos_profile);
    }

private:
    // 동기화된 IMU & LiDAR 메시지가 도착했을 때 호출되는 콜백함수 
    void callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, 
                  const sensor_msgs::msg::PointCloud2::ConstSharedPtr lidar_msg)
    {
        RCLCPP_INFO(this->get_logger(), "Callback function triggered!");

        auto synced_imu = std::make_shared<sensor_msgs::msg::Imu>(*imu_msg);
        auto synced_points = std::make_shared<sensor_msgs::msg::PointCloud2>(*lidar_msg);

        lidar_pub_->publish(*synced_points);
        imu_pub_->publish(*synced_imu);
    }

    
	// 개별 LiDAR 데이터 수신 콜백
    using SyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, sensor_msgs::msg::PointCloud2>;
    std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;

    message_filters::Subscriber<sensor_msgs::msg::Imu> imu_sub_;
    message_filters::Subscriber<sensor_msgs::msg::PointCloud2> lidar_sub_;

    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_pub_;

};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SyncLidarImuNode>());
    rclcpp::shutdown();
    return 0;
}

전체 코드는 위와 같고, 여기서 중요한 2가지가 있습니다.
각 프로그램 마다 적용되는 QoS 정책이 다를 수가 있으니 참고해주세요.

1. QoS 설정 방식

SubscriptionOptions

QoS 충돌 상황에서 강제로 QoS 적용가능. 특히 RELIABLE vs BEST_EFFORT 문제 해결 도움.

2. message_filters::Subscriber의 QoS 처리

imusub.subscribe(this, "/imu/data", rmw_qos_profile_sensor_data);

rmw_qos_profile_sensor_data는 ROS2에서 BEST_EFFORT에 맞게 정의된 기본 QoS
→ 퍼블리셔와 일치하도록 확실하게 맞춰줌.

☑️실행

개발환경 : 우분투 22.04. ROS humble
사용 장비 : OUSTER LIDAR 32채널, VECTORNAV imu VN-100

1. LiDAR launch

ros2 launch ouster_ros sensor.launch.xml

2. IMU 실행

사용 패키지 : imu_vn_100 (※ rolling 브랜치 사용)

ros2 launch imu_vn_100 imu_vn_100-launch.py

3. 동기화 실행

ros2 run imu_vn_100 sync_imu

☑️결과 확인

ros2 bag을 이용한 동기화 확인

ros2 bag record /synced_points /synced_imu

ros2 bag info <bag_file_name>

동기화 전 :
/imu/data 메시지 수 : 561
/ouster/points 메시지 수 : 56

~~동기화 후 :~~
~~/synced_imu 메시지 수 : 43~~
~~/ouster/points 메시지 수 : 45~~

++수정
제가 이전에는 /synced_imu와 /ouster/points와 비교를 했었는데 imu와 points 모두 동기화 이후의 토픽 메시지를 받아보면 메시지 수가 동일하게 나옵니다!

RTK-GPS 사용

yeong77 — Sat, 2 Aug 2025 12:11:38 +0900

Sparkfun사의 ZED-F9P RTK-GPS 사용법을 포스트하겠습니다.

참고한 사이트는 아래와 같습니다.

RTK F9P SOLUTION

Updating the Receiver Firmware

로버 수신기 펌웨어 업데이트
NMEA 축소 > UBX 확장 > MON 확장 > Version 확인
Tools > Firmware
체크해제 'Enter safeboot before update'

Setting Up a Temporary Base

Base를 고정해서 데이터 수신.
Telemetry radio kit 이용해 RTCM 보정 데이터를 rover에 전송.
Telemetry radio가 base에서 rover로 RTCM 통신에 적합.

포트 연결

u-center 열고 보드 연결한 후에 windows가 생성한 해당 COM 포트에 연결.

모듈 구성
1) 라디오-ZED-F9P의 UART2 연결 가정
2) u-center 메시지 창에서 아래의 메시지를 UART2와 USB 모두 활성화
3) 이후 'SEND' 버튼

RTCM 문장만 전송하도록 설정

PRT > UART2로 설정 > 57600으로 설정 > SEND
UART2는 serial RTCM을 보내고 받는데 적합.
USB는 RTCM 메시지가 출력되고 있는지 확인할 수 있도록 활성화됨.

현재 설정 저장

CFG > BBR : 배터리 백업 RAM > FLASH : 설정 플래시 메모리에 기록

Survey in 초기화
1) CFG > TMODE3으로 이동
2) 모드 변경
3) 시간 변경 (관측 시간)
4) 정확도 변경 (필요한 정확도)
5) 'SEND'
Survey 상태 모니터링
1) NAV로 이동
2) SVIN 우클릭후, Enable message 선택
⇾ 모듈이 이 레지스터 상태를 매초마다 보내도록 설정이 됨.
⇾ 60초 동안 데이터 수집하고 표준편차는 5m 이하로 작아진다. (위의 그림으로 예를 들어)
⇾ Survey, “complete” 라고 알림.
⇾ 최종적으로 모듈이 고정됨(fixed).
Base 고정 확인

NAV > PVT
Fix Type이 'TIME'이면 Base를 성공적으로 설정한 것임.

메시지 확인

포켓 콘솔 창열면 RTCM 메시지 확인 가능.

저장 및 로드
1) Tools > Receiver Configuration
2) 현재 설정 파일로 저장(Survey-In-Config.txt.으로 저장)
3) u-center 사용하여 로드
4) 로드 후 BBR/Flash에 기록해야함.

Rover Setup

U-blox에서 ZED-F9P를 로버모드로 설정
로버는 RTCM 보정 데이터를 받으면, RTK-Float / RTK Fix로 진입
u-center 열고 로버 모듈에 연결

[준비]
1. UBX>CFG>PRT : 1. UART2로 구성 2. Baudrate 57600으로 설정 (베이스와 라디오 일치하게)

※ 모듈 설정에 문제 발생하면 ‘Revert to default configuration’ 선택하고 전송

아래와 같이 납땜. (telemtry와 zed-f9p 모듈)

[테스트]
1. Rover와 Base를 전원에 연결 ⇾ Telemetry radio가 서로 인식하고 직렬 데이터 전달.
2. Rover 상태 확인위해 u-center 준비.
⇾ 위치 정확도 NAV > HPPOSECEF
⇾ 수평 정확도 NAV > HPPOSLLH
⇾ 3D에서 RTK Float, RTK Fixed로 진행될 때 고정유형 보여줌 (NAV > PVT)
(Rover가 RTCM 수신하는 동안, 3D Fix > RTK Float > RTK Fix 빠르게 이동)

✔️ RTK LED ON = 일반 위성 모드, 비-RTK 모드
✔️ RTK LED 깜박임 = RTK Float, ~<500mm 정확도
✔️ RTK LED OFF = RTK Fix, 14mm acurracy

로버 RTK Fix 후 ZED-F9P는 UART1과 USB를 통해 고정밀 좌표를 출력.

정리

모듈을 전원에 연결
Base가 Survey in 완료 후 무선 링크를 통해 RTCM 전송 시작
몇초동안 RTCM 수신한 후, Rover의 수신기 RTK Float ⇾ RTK Fix로 진입 후 높 은 정확도의 데이터 출력

다음 사진은 RTK가 고정되었을 때 화면이다.