[ROS2] Behavior-Tree 실습 (C++)

패키지 생성

• src 하위에 생성
• ros2 pkg create bt_example --build-type ament_cmake --dependencies rclcpp behaviortree_cpp_v3
  ㄴ --build-type ament_cmake → C++ 패키지
  ㄴ --dependencies rclcpp behaviortree_cpp_v3 → 필요한 패키지 의존성 지정

패키지 구조 확인

bt_example/
├── CMakeLists.txt
├── package.xml
└── src/
    └── bt_test.cpp (생성할 파일)

CMakeLists.txt 수정

cmake_minimum_required(VERSION 3.8)
project(bt_example)

# 의존성
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(behaviortree_cpp_v3 REQUIRED)

add_executable(bt_test src/bt_test.cpp)

# 의존성 연결
ament_target_dependencies(
  bt_test
  behaviortree_cpp_v3
  rclcpp
)

install(TARGETS
  bt_test
  DESTINATION lib/${PROJECT_NAME})

# ROS2 패키지로 등록
ament_package()

bt_test.cpp 파일 생성

#include <behaviortree_cpp_v3/bt_factory.h>
#include <iostream>

using namespace BT;

// ---- Action 노드 정의 ----
class SayHello : public SyncActionNode
{
    public:
        SayHello(const std::string& name) : SyncActionNode(name, {}) {}
        NodeStatus tick() override
        {
            std::cout << "Hello!" << std::endl;
            return NodeStatus::SUCCESS;
        }
};

class SayBye : public SyncActionNode
{
    public:
        SayBye(const std::string& name) : SyncActionNode(name, {}) {}
        NodeStatus tick() override
        {
            std::cout << "Bye!" << std::endl;
            return NodeStatus::SUCCESS;
        }
};

// ---- Condition 노드 ----
NodeStatus CheckCondition()
{
    // bool flag = false;
    bool flag = true;
    if(flag)
    {
        std::cout << "Condition OK!" << std::endl;
        return NodeStatus::SUCCESS;
    }
    else
    {
        std::cout << "Condition FAIL!" << std::endl;
        return NodeStatus::FAILURE;
    }
}

// ---- 메인 ----
int main()
{
    BehaviorTreeFactory factory;

    // Node 등록
    factory.registerNodeType<SayHello>("SayHello");
    factory.registerNodeType<SayBye>("SayBye");
    factory.registerSimpleCondition("CheckCondition", std::bind(CheckCondition));

    // XML 트리 정의
    static const char* xml_text = R"(
    <root main_tree_to_execute="MainTree">
        <BehaviorTree ID="MainTree">
            <Sequence name="main_sequence">
                <CheckCondition/>
                <SayHello/>
                <Fallback>
                    <SayBye/>
                    <SayHello/>
                </Fallback>
            </Sequence>
        </BehaviorTree>
    </root>
    )";

    auto tree = factory.createTreeFromText(xml_text);

    // 트리 실행
    tree.tickRoot();

    return 0;
}

패키지 빌드

• src 폴더 상위에서 실행
• colcon build --symlink-install --packages-select bt_example
• source install/setup.bash

실행

• ros2 run bt_example bt_test

CheckCondition의 bool flag = true; 일때

ㄴ xml에서 Sequence로 노드들을 묶었으니 차례대로 실행됨을 알 수 있음
ㄴ Sequnce 내부의 Fallback은 SayBye와 SayHello가 있으나 SayBye에서 성공 반환으로 그 이후의 노드는 실행하지 않음

CheckCondition의 bool flag = true; 일때

ㄴ Sequnce로 묶인 노드 중 FAILURE를 만나면 종료됨을 알 수 있음