1296.68 FlyToWaypoint 액션 노드 구현
1. 개요
FlyToWaypoint 액션 노드는 드론(UAV)이 현재 위치에서 지정된 경유점(waypoint)까지 비행하는 동작을 행동 트리 내에서 수행하는 리프 노드이다. 경유점 비행은 드론 자율 임무의 핵심 구성 요소로서, 3차원 공간 내의 목표 좌표(위도, 경도, 고도)로의 이동을 수행한다.
경유점 도달까지의 비행 시간은 거리, 풍속, 비행 속도 등에 따라 수 초에서 수 분까지 다양하므로, BT::StatefulActionNode를 상속하여 비동기 방식으로 구현한다. 노드는 비행 명령 전송 후 RUNNING 상태를 유지하며, 목표 경유점의 수용 반경(acceptance radius) 내에 드론이 진입하면 SUCCESS를 반환한다.
2. 좌표계와 경유점 표현
2.1 지리 좌표계 (Geographic Coordinate System)
드론의 경유점은 일반적으로 WGS84(World Geodetic System 1984) 지리 좌표계에서 위도(latitude), 경도(longitude), 고도(altitude)의 3차원 좌표로 표현한다. ROS2 환경에서는 geographic_msgs/GeoPoint 또는 sensor_msgs/NavSatFix 메시지 타입을 활용하여 지리 좌표를 전달한다.
2.2 지역 좌표계 (Local Coordinate System)
실내 환경이나 제한된 영역에서의 비행에서는 NED(North-East-Down) 또는 ENU(East-North-Up) 지역 좌표계를 사용한다. PX4 자동 비행 소프트웨어는 NED 좌표계를 기본으로 사용하며, ROS2 생태계에서는 REP-103(Standard Units of Measure and Coordinate Conventions)에 따라 ENU 좌표계를 사용한다. 따라서 PX4-ROS2 간의 좌표 변환이 필요하다.
좌표 변환은 다음과 같이 수행한다.
$$
\begin{bmatrix} x_{\text{ENU}} \ y_{\text{ENU}} \ z_{\text{ENU}} \end
\begin{bmatrix} 0 & 1 & 0 \ 1 & 0 & 0 \ 0 & 0 & -1 \end{bmatrix}
\begin{bmatrix} x_{\text{NED}} \ y_{\text{NED}} \ z_{\text{NED}} \end{bmatrix}
$$
3. ROS2 액션 인터페이스 정의
FlyToWaypoint 동작에 대한 ROS2 액션 타입은 목표 경유점 좌표, 비행 속도, 수용 반경 등을 포함하도록 정의한다.
# FlyToWaypoint.action
# Goal
float64 latitude # 목표 위도 (도)
float64 longitude # 목표 경도 (도)
float64 altitude # 목표 고도 (미터, MSL 또는 AGL)
float64 speed # 비행 속도 (m/s)
float64 acceptance_radius # 수용 반경 (미터)
uint8 altitude_reference # 고도 기준 (0: MSL, 1: AGL)
---
# Result
bool success # 도달 성공 여부
float64 final_distance # 최종 잔여 거리 (미터)
uint8 error_code # 오류 코드
---
# Feedback
float64 distance_remaining # 잔여 거리 (미터)
float64 eta # 도착 예상 시간 (초)
float64 current_speed # 현재 비행 속도 (m/s)
float64 current_altitude # 현재 고도 (미터)
acceptance_radius는 경유점 도달 판정의 공간 임계값으로, 드론이 목표 지점으로부터 이 반경 내에 진입하면 경유점에 도달한 것으로 판정한다. 일반적인 실외 GPS 기반 비행에서는 1~5미터, RTK-GPS 기반 정밀 비행에서는 0.1~0.5미터로 설정한다.
4. 클래스 구조 설계
#include <behaviortree_cpp/action_node.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "drone_interfaces/action/fly_to_waypoint.hpp"
class FlyToWaypointAction : public BT::StatefulActionNode
{
public:
using FlyToWaypoint = drone_interfaces::action::FlyToWaypoint;
using GoalHandleFly =
rclcpp_action::ClientGoalHandle<FlyToWaypoint>;
FlyToWaypointAction(const std::string& name,
const BT::NodeConfiguration& config,
rclcpp::Node::SharedPtr node)
: BT::StatefulActionNode(name, config),
node_(node)
{
action_client_ =
rclcpp_action::create_client<FlyToWaypoint>(
node_, "fly_to_waypoint");
}
static BT::PortsList providedPorts()
{
return {
BT::InputPort<double>("latitude",
"목표 위도 (도)"),
BT::InputPort<double>("longitude",
"목표 경도 (도)"),
BT::InputPort<double>("altitude",
"목표 고도 (미터)"),
BT::InputPort<double>("speed", 5.0,
"비행 속도 (m/s)"),
BT::InputPort<double>("acceptance_radius", 2.0,
"수용 반경 (미터)"),
BT::InputPort<double>("timeout", 120.0,
"타임아웃 (초)"),
BT::OutputPort<double>("final_distance",
"최종 잔여 거리")
};
}
BT::NodeStatus onStart() override;
BT::NodeStatus onRunning() override;
void onHalted() override;
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Client<FlyToWaypoint>::SharedPtr
action_client_;
GoalHandleFly::SharedPtr goal_handle_;
std::atomic<bool> goal_completed_{false};
std::atomic<bool> goal_succeeded_{false};
double final_distance_{0.0};
rclcpp::Time start_time_;
};
latitude와 longitude 입력 포트에는 기본값을 설정하지 않는다. 이는 경유점 좌표가 임무에 따라 반드시 명시적으로 지정되어야 하는 필수 파라미터이기 때문이다.
5. 콜백 메서드 구현
5.1 onStart() 콜백
BT::NodeStatus FlyToWaypointAction::onStart()
{
if (!action_client_->wait_for_action_server(
std::chrono::seconds(5)))
{
RCLCPP_ERROR(node_->get_logger(),
"FlyToWaypoint 액션 서버 연결 실패");
return BT::NodeStatus::FAILURE;
}
double latitude, longitude, altitude;
double speed, acceptance_radius;
if (!getInput("latitude", latitude) ||
!getInput("longitude", longitude) ||
!getInput("altitude", altitude))
{
RCLCPP_ERROR(node_->get_logger(),
"경유점 좌표 입력 포트 읽기 실패");
return BT::NodeStatus::FAILURE;
}
getInput("speed", speed);
getInput("acceptance_radius", acceptance_radius);
auto goal_msg = FlyToWaypoint::Goal();
goal_msg.latitude = latitude;
goal_msg.longitude = longitude;
goal_msg.altitude = altitude;
goal_msg.speed = speed;
goal_msg.acceptance_radius = acceptance_radius;
auto send_goal_options =
rclcpp_action::Client<FlyToWaypoint>::SendGoalOptions();
send_goal_options.result_callback =
[this](const%20GoalHandleFly::WrappedResult&%20result)
{
goal_completed_ = true;
goal_succeeded_ =
(result.code ==
rclcpp_action::ResultCode::SUCCEEDED);
if (goal_succeeded_)
{
final_distance_ =
result.result->final_distance;
}
};
send_goal_options.feedback_callback =
[this](GoalHandleFly::SharedPtr,
%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20const%20std::shared_ptr<
%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20const%20FlyToWaypoint::Feedback>%20feedback)
{
RCLCPP_DEBUG(node_->get_logger(),
"잔여 거리: %.1f m, ETA: %.1f s, 속도: %.1f m/s",
feedback->distance_remaining,
feedback->eta,
feedback->current_speed);
};
action_client_->async_send_goal(goal_msg,
send_goal_options);
start_time_ = node_->now();
RCLCPP_INFO(node_->get_logger(),
"경유점 비행 시작: (%.6f, %.6f, %.1f)",
latitude, longitude, altitude);
return BT::NodeStatus::RUNNING;
}
피드백 콜백에서는 잔여 거리, 도착 예상 시간, 현재 속도를 DEBUG 레벨로 기록한다. 이는 정상 운용 시에는 로그 출력을 최소화하면서도, 디버깅 시에는 비행 경과를 상세히 추적할 수 있도록 하기 위함이다.
5.2 onRunning() 콜백
BT::NodeStatus FlyToWaypointAction::onRunning()
{
double timeout;
getInput("timeout", timeout);
auto elapsed = (node_->now() - start_time_).seconds();
if (elapsed > timeout)
{
RCLCPP_WARN(node_->get_logger(),
"FlyToWaypoint 타임아웃 (%.1f초 경과)",
elapsed);
if (goal_handle_)
{
action_client_->async_cancel_goal(goal_handle_);
}
return BT::NodeStatus::FAILURE;
}
if (goal_completed_)
{
if (goal_succeeded_)
{
setOutput("final_distance", final_distance_);
return BT::NodeStatus::SUCCESS;
}
else
{
return BT::NodeStatus::FAILURE;
}
}
return BT::NodeStatus::RUNNING;
}
5.3 onHalted() 콜백
void FlyToWaypointAction::onHalted()
{
if (goal_handle_)
{
RCLCPP_INFO(node_->get_logger(),
"FlyToWaypoint 액션 중단 요청");
action_client_->async_cancel_goal(goal_handle_);
}
goal_completed_ = false;
goal_succeeded_ = false;
goal_handle_.reset();
}
비행 중 onHalted()가 호출되면 드론은 현재 위치에서 체공(loiter) 또는 위치 유지(position hold) 모드로 전환되어야 한다. 이 전환은 비행 제어 시스템의 액션 서버 측에서 골 취소 수신 시 처리한다.
6. 경유점 도달 판정
6.1 Haversine 공식 기반 거리 계산
지리 좌표계에서 두 점 사이의 대원 거리(great-circle distance)를 계산하기 위해 Haversine 공식을 사용한다.
a = \sin^2\!\left(\frac{\Delta\varphi}{2}\right) + \cos(\varphi_1)\cos(\varphi_2)\sin^2\!\left(\frac{\Delta\lambda}{2}\right)
d = 2R \cdot \text{atan2}\!\left(\sqrt{a},\;\sqrt{1-a}\right)
여기서 \varphi는 위도(라디안), \lambda는 경도(라디안), R은 지구 평균 반지름(6,371,000 m)이다. 계산된 수평 거리 d와 수직 거리 \Delta h로부터 3차원 유클리드 거리를 산출하여 수용 반경과 비교한다.
d_{3D} = \sqrt{d^2 + (\Delta h)^2}
6.2 수용 반경 설정 기준
수용 반경은 드론의 위치 추정 정밀도, 비행 속도, 풍속 조건에 따라 적절히 설정해야 한다.
| 위치 추정 방식 | 수평 정밀도 (CEP) | 권장 수용 반경 |
|---|---|---|
| 단독 GPS | 2~5 m | 3~10 m |
| DGPS | 0.5~2 m | 1~3 m |
| RTK-GPS | 0.01~0.02 m | 0.1~0.5 m |
| 비전 기반 | 0.1~0.5 m | 0.3~1.0 m |
수용 반경을 위치 추정 정밀도보다 작게 설정하면, 측정 잡음(measurement noise)으로 인해 경유점 도달을 판정하지 못하고 목표 주변을 반복적으로 선회하는 현상이 발생할 수 있다.
7. 비행 경로 제어
7.1 직선 비행 (Straight-Line Flight)
가장 기본적인 경유점 비행 방식은 현재 위치에서 목표 경유점까지 직선 경로를 추종하는 것이다. 비행 제어기의 위치 제어 루프가 L1 유도 법칙(L1 guidance law) 또는 순수 추적(pure pursuit) 알고리즘을 사용하여 경로를 추종한다.
L1 유도 법칙은 다음과 같이 정의된다.
a_{\text{cmd}} = \frac{2V^2 \sin(\eta)}{L_1}
여기서 V는 대기 속도, \eta는 드론의 진행 방향과 L1 벡터 사이의 각도, L_1은 전방 참조 거리(look-ahead distance)이다.
7.2 곡선 비행 (Curved Flight)
경유점 간의 전환 시 급격한 방향 전환을 피하기 위해, 더부스-경로(Dubins path) 또는 곡률 제한 경로를 사용할 수 있다. 이러한 경로 계획은 FlyToWaypoint 노드 내부가 아닌 별도의 경로 계획 모듈에서 수행하는 것이 단일 책임 원칙에 부합한다.
8. XML 행동 트리에서의 활용
8.1 노드 등록
factory.registerBuilder<FlyToWaypointAction>(
"FlyToWaypoint",
[ros_node](const%20std::string&%20name,
%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20const%20BT::NodeConfiguration&%20config)
{
return std::make_unique<FlyToWaypointAction>(
name, config, ros_node);
});
8.2 다중 경유점 비행 임무
복수의 경유점을 순차적으로 비행하는 임무는 Sequence 제어 노드를 활용하여 구성한다.
<root BTCPP_format="4">
<BehaviorTree ID="WaypointMission">
<Sequence>
<Takeoff target_altitude="20.0" />
<FlyToWaypoint latitude="37.5665"
longitude="126.9780"
altitude="50.0"
speed="8.0"
acceptance_radius="3.0"
timeout="180.0" />
<FlyToWaypoint latitude="37.5700"
longitude="126.9820"
altitude="50.0"
speed="8.0"
acceptance_radius="3.0"
timeout="180.0" />
<FlyToWaypoint latitude="37.5680"
longitude="126.9850"
altitude="50.0"
speed="5.0"
acceptance_radius="1.5"
timeout="120.0" />
<ReturnToHome />
<Land descent_rate="1.5" />
</Sequence>
</BehaviorTree>
</root>
8.3 블랙보드를 통한 동적 경유점 지정
임무 계획기(mission planner)가 블랙보드에 경유점 좌표를 동적으로 기록하면, FlyToWaypoint 노드는 이를 읽어 비행을 수행할 수 있다.
<FlyToWaypoint latitude="{next_wp_lat}"
longitude="{next_wp_lon}"
altitude="{next_wp_alt}"
speed="{cruise_speed}" />
이 방식은 실행 시점에 경유점이 결정되는 동적 임무 계획에 적합하며, XML 행동 트리를 재정의하지 않고도 임무 경로를 변경할 수 있는 유연성을 제공한다.
9. 안전성 고려 사항
- 지오펜스 검사: 목표 경유점이 사전 정의된 지오펜스(geofence) 범위 내에 있는지를 비행 전에 검증해야 한다. 지오펜스 위반 시
FAILURE를 반환하여 상위 행동 트리에서 대체 경로를 탐색하도록 한다. - 충돌 회피: 비행 경로 상의 장애물 회피는 FlyToWaypoint 노드의 범위를 벗어나며, 비행 제어 시스템 또는 별도의 충돌 회피 모듈에서 처리해야 한다.
- 비행 고도 제한: 항공법규에 따른 최대 비행 고도 제한(예: 대한민국 항공안전법 기준 150미터)을 준수해야 한다.
- 배터리 잔량 기반 귀환 판단: 목표 경유점까지의 비행과 귀환에 필요한 배터리 잔량을 사전에 산정하여, 부족할 경우 비행을 시작하지 않아야 한다.
10. 참고 문헌
- Colledanchise, M. and Ögren, P., “Behavior Trees in Robotics and AI: An Introduction,” CRC Press, 2018.
- Park, S. et al., “A New Nonlinear Guidance Logic for Trajectory Tracking,” AIAA Guidance, Navigation, and Control Conference, 2004.
- Meier, L. et al., “PX4: A Node-Based Multithreaded Open Source Robotics Framework for Deeply Embedded Platforms,” IEEE International Conference on Robotics and Automation (ICRA), 2015.
- Faconti, D. and Contributors, “BehaviorTree.CPP: A C++ library to build Behavior Trees,” GitHub Repository, https://github.com/BehaviorTree/BehaviorTree.CPP.
- Sinnott, R. W., “Virtues of the Haversine,” Sky and Telescope, vol. 68, no. 2, p. 159, 1984.
버전: 2026-04-04