1296.60 BackUp 액션 노드 구현

1. BackUp 액션 노드의 정의와 역할

BackUp은 Nav2 프레임워크에서 제공하는 복구 행동(recovery behavior) 액션 노드로, 로봇을 지정된 거리만큼 후진시키는 임무를 수행한다. 이 노드는 nav2_msgs::action::BackUp ROS2 액션 인터페이스의 클라이언트로 동작하며, behavior_server에 후진 요청을 전송한다.

BackUp 행동의 주된 목적은 로봇이 전방 장애물에 의해 진행이 차단된 상황에서 후진하여 기동 공간을 확보하는 것이다. 다음과 같은 복구 시나리오에서 활용된다.

• 좁은 통로에서 전방 장애물과의 교착(deadlock) 상태 해소
• 경로 추종 실패 후 안전한 위치로의 후퇴
• 제자리 회전이 불가능한 공간에서의 방향 전환을 위한 전처리

2. 액션 인터페이스 정의

# nav2_msgs/action/BackUp

# Goal
geometry_msgs/Point target
float32 speed
float32 time_allowance
---
# Result
float32 total_elapsed_time
---
# Feedback
float32 distance_traveled

골(Goal) 필드:

• target: 후진할 목표 변위 벡터. target.x는 후진 거리 [m]를 나타내며, 음수 값을 사용한다(로봇 기준 후방 이동이므로).
• speed: 후진 속도 [m/s]. 음수 값을 사용한다.
• time_allowance: 후진 동작에 허용되는 최대 시간 [s].

결과(Result) 필드:

• total_elapsed_time: 후진 동작에 소요된 총 시간 [s].

피드백(Feedback) 필드:

• distance_traveled: 현재까지 후진한 거리 [m].

3. 클래스 구조

#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/back_up.hpp"

namespace nav2_behavior_tree
{

class BackUpAction
  : public BtActionNode<nav2_msgs::action::BackUp>
{
public:
  BackUpAction(
    const std::string & xml_tag_name,
    const std::string & action_name,
    const BT::NodeConfiguration & conf);

  void on_tick() override;

  void on_wait_for_result(
    std::shared_ptr<const nav2_msgs::action::BackUp::Feedback>
      feedback) override;

  BT::NodeStatus on_success() override;
  BT::NodeStatus on_aborted() override;
  BT::NodeStatus on_cancelled() override;

  static BT::PortsList providedPorts();
};

}  // namespace nav2_behavior_tree

4. 포트 정의

BT::PortsList BackUpAction::providedPorts()
{
  return providedBasicPorts({
    BT::InputPort<double>(
      "backup_dist", 0.3,
      "Distance to back up [m]"),
    BT::InputPort<double>(
      "backup_speed", 0.025,
      "Speed of back up [m/s]"),
    BT::InputPort<double>(
      "time_allowance", 10.0,
      "Maximum time allowed [s]"),
    BT::OutputPort<int>(
      "error_code", "Error code on failure")
  });
}

backup_dist는 후진할 거리를 양수 값으로 지정한다. 내부적으로 음수로 변환되어 로봇 후방 이동을 지시한다. 기본값은 0.3 m이다. backup_speed는 후진 속도를 양수 값으로 지정하며, 기본값은 0.025 m/s이다. 낮은 속도는 후진 중의 안전성을 보장한다.

5. on_tick() 구현

void BackUpAction::on_tick()
{
  double backup_dist, backup_speed, time_allowance;
  getInput("backup_dist", backup_dist);
  getInput("backup_speed", backup_speed);
  getInput("time_allowance", time_allowance);

  // 후진 방향이므로 음수로 변환
  goal_.target.x = -backup_dist;
  goal_.target.y = 0.0;
  goal_.target.z = 0.0;
  goal_.speed = -backup_speed;
  goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
}

target.x와 speed를 음수로 설정하여 로봇 좌표계에서의 후방 이동을 지시한다. ROS2의 관례에 따라 로봇 좌표계의 x축은 전방을 향하므로, 음수 x 값은 후방 이동에 해당한다.

6. on_wait_for_result() 구현

void BackUpAction::on_wait_for_result(
  std::shared_ptr<const nav2_msgs::action::BackUp::Feedback> feedback)
{
  RCLCPP_DEBUG(
    node_->get_logger(),
    "BackUp: distance traveled = %.3f m",
    feedback->distance_traveled);
}

7. 결과 처리 콜백 구현

BT::NodeStatus BackUpAction::on_success()
{
  return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus BackUpAction::on_aborted()
{
  setOutput("error_code",
    static_cast<int>(result_.result->error_code));
  return BT::NodeStatus::FAILURE;
}

BT::NodeStatus BackUpAction::on_cancelled()
{
  return BT::NodeStatus::SUCCESS;
}

BackUp이 실패하는 주요 원인은 다음과 같다.

• 후방에 장애물이 존재하여 후진이 불가능한 경우
• 허용 시간 내에 목표 거리에 도달하지 못한 경우
• 후진 중 코스트맵의 치명적 영역(lethal area)에 진입한 경우

8. XML 행동 트리에서의 사용

8.1 단독 사용

<BackUp backup_dist="0.3"
        backup_speed="0.025"
        time_allowance="10.0"
        server_name="backup"
        server_timeout="5"/>

8.2 복구 행동 시퀀스 내 사용

BackUp은 다른 복구 행동과 조합하여 계층적 복구 전략을 구성한다.

<SequenceStar>
  <ClearEntireCostmap
    service_name="/local_costmap/clear_entirely_local_costmap"/>
  <BackUp backup_dist="0.3" backup_speed="0.025"/>
  <Spin spin_dist="1.57"/>
  <Wait wait_duration="3.0"/>
</SequenceStar>

이 구성은 코스트맵 초기화 → 후진 → 제자리 회전 → 대기의 순서로 복구를 시도한다. SequenceStar를 사용하여 이전에 성공한 단계를 건너뛰고, 실패한 단계부터 재시도한다.

8.3 RecoveryNode 내 사용

<RecoveryNode number_of_retries="2">
  <Sequence>
    <ComputePathToPose goal="{target}" path="{path}"/>
    <FollowPath path="{path}"/>
  </Sequence>
  <Sequence>
    <BackUp backup_dist="0.5" backup_speed="0.05"/>
    <ClearEntireCostmap
      service_name="/global_costmap/clear_entirely_global_costmap"/>
  </Sequence>
</RecoveryNode>

9. 서버 측 동작 원리

behavior_server의 BackUp 플러그인은 다음의 과정을 통해 후진을 실행한다.

1. 골 수신 시 로봇의 현재 위치를 기록한다.
2. 지정된 속도로 후방 이동을 위한 cmd_vel 메시지를 발행한다. 선속도(linear.x)를 음수로 설정한다.
3. 매 제어 주기마다 로봇의 현재 위치를 측정하여 이동 거리를 계산한다.
4. 이동 거리가 목표 거리에 도달하면 정지 명령을 발행하고 SUCCESS를 반환한다.
5. 후방 장애물과의 충돌이 감지되면 정지 명령을 발행하고 ABORTED를 반환한다.

10. 충돌 검사

BackUp 동작 중 서버는 로봇의 후방 경로 상에 장애물이 존재하는지 지속적으로 검사한다. 코스트맵의 장애물 데이터와 로봇의 풋프린트를 사용하여 후진 궤적의 안전성을 평가한다. 후방에 장애물이 감지되면 후진을 즉시 중단하여 충돌을 방지한다.

후방 장애물 검사의 정확도는 코스트맵의 갱신 빈도와 센서의 후방 감지 범위에 의존한다. 전방만 감지 가능한 센서 구성(단일 전방 LiDAR 등)에서는 후방 장애물 정보가 코스트맵에 반영되지 않을 수 있으므로, 후방 센서(초음파 센서, 후방 카메라 등)의 추가 장착을 고려하여야 한다.

11. 후진 거리와 속도 설정

11.1 후진 거리

후진 거리는 복구 목적에 따라 적절히 설정한다. 일반적인 지침은 다음과 같다.

• 최소 복구 거리(0.1~0.3 m): 경로 이탈 후 경로 재진입에 필요한 최소 공간 확보
• 표준 복구 거리(0.3~0.5 m): 전방 장애물과의 안전 거리 확보
• 최대 복구 거리(0.5~1.0 m): 교착 상태 해소를 위한 충분한 기동 공간 확보

과도한 후진 거리는 이미 탐색된 영역으로의 불필요한 후퇴를 야기하므로, 상황에 적합한 최소 거리를 설정하는 것이 바람직하다.

11.2 후진 속도

후진 속도는 안전성과 효율성의 균형을 고려하여 설정한다. 후진 중에는 로봇의 전방 센서가 이동 방향을 감지하지 못하므로, 전진 속도보다 현저히 낮은 속도를 사용한다. 일반적으로 최대 전진 속도의 10~20% 수준이 권장된다.

12. 비홀로노믹 로봇에서의 고려 사항

자동차형 조향 메커니즘을 가진 비홀로노믹 로봇에서 후진은 조향 방향이 반전되어 제어 특성이 전진과 상이하다. 이러한 로봇에서는 후진 속도를 더욱 보수적으로 설정하고, 후진 거리를 최소화하며, 후진 중 조향 각도를 제한하는 것이 권장된다.

차동 구동(differential drive) 로봇은 선속도의 부호만 반전하면 되므로 후진 제어가 단순하지만, 전방향(omnidirectional) 로봇은 임의 방향 이동이 가능하므로 BackUp 대신 측면 이동을 고려할 수도 있다.

13. 플러그인 등록

#include "behaviortree_cpp/bt_factory.h"

BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const%20std::string%20&%20name,%20const%20BT::NodeConfiguration%20&%20config)
    {
      return std::make_unique<nav2_behavior_tree::BackUpAction>(
        name, "backup", config);
    };
  factory.registerBuilder<nav2_behavior_tree::BackUpAction>(
    "BackUp", builder);
}

참고 문헌 및 출처

• Macenski, S., Martín, F., White, R., & Clavero, J. G. (2020). “The Marathon 2: A Navigation System.” arXiv preprint arXiv:2003.00368.
• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Nav2 공식 문서 — Behavior Server: https://docs.nav2.org/configuration/packages/configuring-behavior-server.html
• BehaviorTree.CPP 공식 문서: https://www.behaviortree.dev/

버전 정보: Nav2 Humble Hawksbill (ROS2 Humble) 기준, BehaviorTree.CPP v3.8 이상.