#3 uXRCE-DDS와 ROS2를 활용한 드론 경로 가시화

이전 게시글에서는 MAVlink Router를 이용해 Endpoint에서 MAVlink 데이터를 주고받는 방법에 대해 알아봤습니다.
이번에는, MAVlink 데이터를 ROS 토픽처럼 볼 수 있는 uXRCE-DDS에 대해 알아보고 드론의 MAVlink 데이터를 ROS2 토픽으로 변환해 이를 Rviz로 가시화해보겠습니다.

환경
Ubuntu 20.04
ROS2 Foxy
PX4 1.15.0
uXRCE-DDS 2.4.3
px4_msgs 1.15

---

1. uXRCE-DDS

PX4는 MAVlink 데이터를 통해 드론의 상태를 알릴 수 있고, 명령과 제어가 가능합니다.
가만히 살펴보면, MAVlink의 데이터 통신 방식은 ROS에서 데이터 통신방식과 개념이 비슷합니다.

MAVlink uORB 메시지를 마치  ROS의 Topic처럼 Publish 하고 Subscribe 할 수 있도록 하는 프로토콜이 uXRCE-DDS입니다.

 Client는 기본적으로 PX4 펌웨어에 포함되어 있으므로 Agent를 실행하여 Topic처럼 볼 수 있습니다. 

설치

git clone -b v2.4.3 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
cd Micro-XRCE-DDS-Agent
mkdir build
cd build
cmake ..
make
sudo make install
sudo ldconfig /usr/local/lib/

 

설치가 완료되면, Gazebo와 XRCE-DDS를 실행해 ROS2 Topic이 발행되는 모습을 확인해 봅시다.

#터미널 1
MicroXRCEAgent udp4 -p 8888

#터미널2
cd ~/PX4-Autopilot
make px4_sitl gazebo-classic

#터미널3
ros2 topic list

fmu/in
fmu/out으로 ROS2 Topic이 발행되고 있는 모습을 확인할 수 있습니다.

---

2. ROS2를 활용한 드론 경로 가시화

간단하게, ROS2 토픽으로 변환된 MAVlink 데이터를 Subscribe 하고 데이터를 가공해 드론의 경로를 가시화해봅시다.

PX4에 ROS2를 이용하려면 메시지 정의가 이루어져야 합니다.
ROS2 노드와 PX4 구성요소들을 연결하기 위해서 px4_msgs를 사용합니다.

#ros2 workspace 생성
mkdir ros_workspace
cd ros_workspace

#px4_msgs v1.15 소스코드 가져오기 
git clone https://github.com/PX4/px4_msgs.git
cd px4_msgs
git checkout release/1.15
cd ..

#의존성 설치 및 빌드
rosdep update
rosdep install --from-paths src --ignore-src -y
colcon build --packages-select px4_msgs --symlink-install

#이후 환경 설정 로드
source install/setup.bash

cd ~/ros_workspace/src

#패키지 생성
ros2 pkg create --build-type ament_python drone_viz_pkg --dependencies rclpy px4_msgs geometry_msgs nav_msgs
cd ~/ros_workspace/src/drone_viz_pkg/drone_viz_pkg/
nano trajectory_node.py

이후 프로그램을 작성합니다.

이때, px4는 NED좌표계를 사용하고, Rviz는 ENU좌표계를 사용하므로 드론의 위치를 적절히 변환하여 입력하도록 합니다.

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from px4_msgs.msg import VehicleLocalPosition
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path

class DroneTrajectoryVisualizer(Node):
    def __init__(self):
        super().__init__('drone_trajectory_visualizer')

        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        self.subscription = self.create_subscription(
            VehicleLocalPosition,
            '/fmu/out/vehicle_local_position',
            self.position_callback,
            qos_profile)

        self.pose_pub = self.create_publisher(PoseStamped, '/drone_pose', 10)
        self.path_pub = self.create_publisher(Path, '/drone_path', 10)
        self.path_msg = Path()

    def position_callback(self, msg):
        self.get_logger().info(f"Received: x={msg.x:.2f}, y={msg.y:.2f}")

        pose_msg = PoseStamped()
        pose_msg.header.stamp = self.get_clock().now().to_msg()
        pose_msg.header.frame_id = "map"
        
        pose_msg.pose.position.x = msg.y
        pose_msg.pose.position.y = msg.x
        pose_msg.pose.position.z = -msg.z
        pose_msg.pose.orientation.w = 1.0

        self.pose_pub.publish(pose_msg)
        self.path_msg.header = pose_msg.header
        self.path_msg.poses.append(pose_msg)
        self.path_pub.publish(self.path_msg)

def main(args=None):
    rclpy.init(args=args)
    node = DroneTrajectoryVisualizer()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

#실행 권환 부여
chmod +x trajectory_node.py

이후 setup.py를 수정합니다.

#~/ros_workspace/src/drone_viz_pkg/setup.py
entry_points={
        'console_scripts': [
            'trajectory_node = drone_viz_pkg.trajectory_node:main'
        ],
    },

워크스페이스로 이동해 빌드합니다.

cd ~/ros_workspace
colcon build --packages-select drone_viz_pkg --symlink-install
source install/setup.bash

정상적으로 빌드가 됐다면,
실행해 봅시다.

#터미널1
ros2 run drone_viz_pkg trajectory_node

#터미널2
MicroXRCEAgent udp4 -p 8888

#터미널3
cd ~/PX4-Autopilot
make px4_sitl gazebo-classic

#터미널4
rviz2

저는 이쪽에 QGC를 띄워 조이스틱을 이용해 드론을 조작해 봤습니다.

 

정상적으로 rviz에서 드론의 경로와 자세가 가시화되는 모습을 확인할 수 있습니다.