ROS2 with CPP - Introduction to Behavior Trees
본 포스트는 ROS1에 대한 전반적인 내용 및 ROS2 Python에 대한 지식이 있다는 가정 하에 작성되었으며, The Construct Youtube 영상을 참고하였습니다.
또한, 시뮬레이션 환경은 turtlebot3 waffle_pi를 사용합니다 : https://github.com/ROBOTIS-GIT/turtlebot3_simulations
BehaviorTree.CPP 설치방법
ros2_ws/src에서 아래 경로 git clone하고 그냥 바로 빌드하면 됩니다.
$ git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git
BT ROS도 같이 깔아줍니다.
$ git clone https://github.com/BehaviorTree/BehaviorTree.ROS2.git
1. Introduction
BT의 구조를 전체적인 그림으로 보면 다음과 같습니다.
지금 보면 이게 대체 뭔 소리인가... 싶을텐데, 일단 그냥 지금은 C++코드에서 저런 구조로 코드가 만들어진다. 정도로만 생각하고 넘어가겠습니다.
아무튼 BT가 꼭 ROS에서만 사용되는건 아니고, 우리가 뭘 하든 논리적인 연결을 필요로 하는 구조가 있다면, BT를 사용할 수 있습니다.
위 그림에서 화살표 기호 "→"는 논리적으로 AND(Sequence), "?"는 논리적으로 OR(Fallback)을 의미합니다. 이 논리 연산들을 사용해서, 우리는 어떻게 로봇이 행동하는지를 만들어낼 수 있습니다. 위 그림의 경우, 로봇은 공을 찾고, 집고, 내려놓습니다. 여기서 집는 동작은 특정한 액션들을 실행하는 것을 필요로 합니다.
즉, "branch of tree"를 성공하기 위해서는 Sequence block하의 모든 액션들이 전부 다 성공해야 합니다. 하지만, Fallback block에서는 단지 하나의 액션만 성공해도 됩니다.
로봇 어플리케이션의 BT를 만들기 위해서는, 먼저 로봇의 행동들을 논리적으로 어떻게 연결할 것인지를 정의해야 합니다. 우리는 이를 위해 XML파일을 사용할 것입니다. nodes, classes, functions은 모두 CPP framework에 정의되어 있습니다.
2. Concept of Behavior Trees
BT는 controll flow라고 하는 core nodes와 execution nodes라고 하는 leaf nodes로 이루어진 directed root tree입니다. 일반적으로 각 node에 대해 부모(parent)와 자식(child)이라고 하기도 합니다. 여기서 root node란, 자신은 부모가 없고, 다른 node들은 하나의 부모만 있는 경우를 의미합니다. 각각의 control flow node에는 최소한 하나의 자식 node가 있어야 합니다.
먼저, Sequence(AND 연산)에 대해 알아보겠습니다.
다음은 tick, callback flow에 대한 그림입니다.
root에서 각각의 child에 tick이라는 신호를 보내는 것이라고 보면 됩니다. 상단 그림의 경우, 모든 child가 success이기 때문에 root도 success이지만, 아래 그림의 경우에는 Task2에서 Failure상태이기 때문에 Task 3에는 tick이 가지 않고, 그냥 Task2에서의 Failure를 받고 root도 바로 Failure가 된 것입니다. AND 연산이기 때문에... 어차피 뒷 부분은 볼 필요도 없는 것입니다
위 그림을 XML로 기술하면 다음과 같습니다. child node들이 Sequence 태그에 들어가 있습니다.
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<RobotTask1 name="task1"/>
<RobotTask2 name="task2"/>
<RobotTask3 name="task3"/>
</Sequence>
</BehaviorTree>
</root>
위 그림을 cpp코드로 구현해보기위해, 먼저 패키지를 만들어봅니다.
$ ros2 pkg create --build-type ament_cmake bt_practice_pkg --dependencies rclcpp std_msgs behaviortree_cpp behaviortree_ros2
위 그림을 cpp 코드로 만들면 다음과 같습니다.
bt_u2_ex2.cpp
#include "behaviortree_cpp/bt_factory.h"
using namespace BT;
class RobotTask1 : public BT::SyncActionNode
{
public:
RobotTask1(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "RobotTask1: " << this->name() << std::endl;
return BT::NodeStatus::FAILURE;
}
};
class RobotTask2 : public BT::SyncActionNode
{
public:
RobotTask2(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "RobotTask2: " << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
class RobotTask3 : public BT::SyncActionNode
{
public:
RobotTask3(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "RobotTask3: " << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
static const char* xml_text = R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<RobotTask1 name="task1"/>
<RobotTask2 name="task2"/>
<RobotTask3 name="task3"/>
</Sequence>
</BehaviorTree>
</root>
)";
// clang-format on
int main()
{
// We use the BehaviorTreeFactory to register our custom nodes
BehaviorTreeFactory factory;
factory.registerNodeType<RobotTask1>("RobotTask1");
factory.registerNodeType<RobotTask2>("RobotTask2");
factory.registerNodeType<RobotTask3>("RobotTask3");
// Trees are created at deployment-time (i.e. at run-time, but only once at the beginning).
// The currently supported format is XML.
// IMPORTANT: when the object "tree" goes out of scope, all the TreeNodes are destroyed
auto tree = factory.createTreeFromText(xml_text);
// To "execute" a Tree you need to "tick" it.
// The tick is propagated to the children based on the logic of the tree.
// In this case, the entire sequence is executed, because all the children
// of the Sequence return SUCCESS.
tree.tickWhileRunning();
return 0;
}
CMakeListst.txt에 아래 내용을 추가합니다
add_executable(bt_u2_ex2 src/bt_u2_ex2.cpp)
ament_target_dependencies(bt_u2_ex2 rclcpp std_msgs behaviortree_cpp behaviortree_ros2)
install(TARGETS
bt_u2_ex2
DESTINATION lib/${PROJECT_NAME}
)
빌드 후, 실행해봅니다. (Warning 문구는 무시하셔도 됩니다.)
class RobotTask1 : public BT::SyncActionNode
{
public:
RobotTask1(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "RobotTask1: " << this->name() << std::endl;
return BT::NodeStatus::FAILURE;
}
};지금 이 녀석이 첫 번째 child node인데, FAILURE를 return했으므로 터미널에는 task1 밖에 보이지 않습니다.
다음으로 Fallback(OR 연산)에 대해 알아보겠습니다.
Fallback은 child로부터 Success또는 Running을 리턴 받을 때 까지 tick을 보냅니다. 여기서 주의해야 할 점은, OR이기 때문에 Success가 하나라도 들어오면, 그 나머지 child에게는 tick을 보내지 않습니다.
즉, 다음 그림과 같습니다.
XML로 나타내면 다음과 같습니다. 이번에는 child node들이 <Fallback> 태그에 들어가 있습니다.
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Fallback name="root_sequence">
<RobotTask1 name="task1"/>
<RobotTask2 name="task2"/>
<RobotTask3 name="task3"/>
</Fallback>
</BehaviorTree>
</root>
다음과 같이 cpp파일을 만들고, CMakeLists를 수정하고 실행해봅니다.
bt_u2_ex3.cpp
#include "behaviortree_cpp/bt_factory.h"
using namespace BT;
class RobotTask1 : public BT::SyncActionNode
{
public:
RobotTask1(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "RobotTask1: " << this->name() << std::endl;
return BT::NodeStatus::FAILURE;
}
};
class RobotTask2 : public BT::SyncActionNode
{
public:
RobotTask2(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "RobotTask2: " << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
class RobotTask3 : public BT::SyncActionNode
{
public:
RobotTask3(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "RobotTask3: " << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
static const char* xml_text = R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Fallback name="root_sequence">
<RobotTask1 name="task1"/>
<RobotTask2 name="task2"/>
<RobotTask3 name="task3"/>
</Fallback>
</BehaviorTree>
</root>
)";
int main()
{
// We use the BehaviorTreeFactory to register our custom nodes
BehaviorTreeFactory factory;
factory.registerNodeType<RobotTask1>("RobotTask1");
factory.registerNodeType<RobotTask2>("RobotTask2");
factory.registerNodeType<RobotTask3>("RobotTask3");
auto tree = factory.createTreeFromText(xml_text);
tree.tickWhileRunning();
return 0;
}
3. BT in Action
이번에는 간단한 BT를 통해 signal flow를 이해해보겠습니다. "tick"은 트리의 root node로부터 leaf node에 닿을때까지 진행하게 됩니다. TreeNode의 callback은 바로 이 tick을 받는 순간에 실행됩니다. 이 callback에는 3가지 종류가 있는데, SUCCESS, FAILURE, RUNNING 입니다.
이번에 사용해볼 tree는 다음과 같습니다.
이번 예시에서 "Eat banana"라는 action은 callback으로 RUNNING을 리턴하는데, 지금 이 경우에는 eating process와 시간을 필요로 합니다. 말로 설명하면 좀 그런데, 다음 결과를 보면 조금 감이 올거에요...
아무튼 우리는 이 것을 asynchronous action이라고 합니다. 일반적으로, BT를 쓰는 주요한 이유는 asynchronous node를 여러 쓰레드에서 돌리기 위함입니다.
위 구조를 XML로 나타내면 다음과 같습니다.
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ReactiveFallback name="root">
<EatSandwich name="eat_sandwich"/>
<EatApple name="eat_apple"/>
<Sequence>
<OpenBanana name="open_banana"/>
<EatBanana goal="1;2;3"/>
<SaySomething message="banan is gone!" />
</Sequence>
</ReactiveFallback>
</BehaviorTree>
</root>
bt_u2_ex4.cpp
#define USE_BTCPP3_OLD_NAMES
#include "behaviortree_cpp/bt_factory.h"
using namespace BT;
class EatSandwich : public BT::SyncActionNode
{
public:
EatSandwich(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "EatSandwich: " << this->name() << std::endl;
return BT::NodeStatus::FAILURE;
}
};
class EatApple : public BT::SyncActionNode
{
public:
EatApple(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "EatApple: " << this->name() << std::endl;
return BT::NodeStatus::FAILURE;
}
};
class OpenBanana : public BT::SyncActionNode
{
public:
OpenBanana(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
std::cout << "OpenBanana: " << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
class EatAction : public BT::AsyncActionNode
{
public:
// Any TreeNode with ports must have a constructor with this signature
EatAction(const std::string& name, const BT::NodeConfiguration& config)
: AsyncActionNode(name, config)
{
}
// It is mandatory to define this static method.
static BT::PortsList providedPorts()
{
return {};
}
BT::NodeStatus tick() override
{
printf("[ EatBanana: STARTED ]");
_halt_requested.store(false);
int count = 0;
// Pretend that "computing" takes 250 milliseconds.
// It is up to you to check periodically _halt_requested and interrupt
// this tick() if it is true.
while (!_halt_requested && count++ < 25)
{
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
std::cout << "[ EatBanana: FINISHED ]" << std::endl;
return _halt_requested ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
}
virtual void halt() override;
private:
std::atomic_bool _halt_requested;
};
void EatAction::halt()
{
_halt_requested.store(true);
}
class SaySomething : public BT::SyncActionNode
{
public:
SaySomething(const std::string& name, const BT::NodeConfiguration& config)
: BT::SyncActionNode(name, config)
{
}
// You must override the virtual function tick()
NodeStatus tick() override
{
auto msg = getInput<std::string>("message");
if (!msg)
{
throw BT::RuntimeError("missing required input [message]: ", msg.error());
}
std::cout << "Robot says: " << msg.value() << std::endl;
return BT::NodeStatus::SUCCESS;
}
// It is mandatory to define this static method.
static BT::PortsList providedPorts()
{
return {BT::InputPort<std::string>("message")};
}
};
static const char* xml_text_reactive = R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ReactiveFallback name="root">
<EatSandwich name="eat_sandwich"/>
<EatApple name="eat_apple"/>
<Sequence>
<OpenBanana name="open_banana"/>
<EatBanana name="eat_banana"/>
<SaySomething message="mission completed!" />
</Sequence>
</ReactiveFallback>
</BehaviorTree>
</root>
)";
// clang-format on
void Assert(bool condition)
{
if (!condition)
throw RuntimeError("this is not what I expected");
}
int main()
{
using std::chrono::milliseconds;
BehaviorTreeFactory factory;
factory.registerNodeType<EatSandwich>("EatSandwich");
factory.registerNodeType<EatApple>("EatApple");
factory.registerNodeType<OpenBanana>("OpenBanana");
factory.registerNodeType<EatAction>("EatBanana");
factory.registerNodeType<SaySomething>("SaySomething");
std::cout << "\n------------ BUILDING A NEW TREE ------------" << std::endl;
auto tree = factory.createTreeFromText(xml_text_reactive);
NodeStatus status;
std::cout << "\n--- 1st executeTick() ---" << std::endl;
status = tree.tickOnce();
Assert(status == NodeStatus::RUNNING);
std::this_thread::sleep_for(std::chrono::milliseconds(150));
std::cout << "\n--- 2nd executeTick() ---" << std::endl;
status = tree.tickOnce();
Assert(status == NodeStatus::RUNNING);
std::this_thread::sleep_for(std::chrono::milliseconds(150));
std::cout << "\n--- 3rd executeTick() ---" << std::endl;
status = tree.tickOnce();
Assert(status == NodeStatus::SUCCESS);
std::cout << std::endl;
// }
return 0;
}
실행 결과는 다음과 같습니다
4. BT Node notation
이번에는 auxiliary node라는 것에 대해 알아보겠습니다. BT에서는 action node가 tick을 받을 때마다 이를 통해 명령이 전달됩니다. 만약 어떤 action이 성공적으로 완료되면 Success를, 아니면 Failure를 리턴합니다. 또한, 아직 처리중이라면 Running을 리턴합니다.
Condition node는 tick을 받을때마다 proposition을 평가합니다. 그 후, proposition이 true인지 false인지에 따라 Success 또는 Failure를 리턴합니다. 참고로, Condition node은 Running을 리턴하지 않습니다.
Decorator node는 단일 child가 있는 control flow node인데, 이전에 정의된 규칙에 따라 child node를 처리하거나 업데이트 할 지를 결정할 수 있습니다. 또한 child가 리턴해준 상태도 그 규칙에 따라 수정할 수 있습니다.
BT node 타입에 따른 정의는 다음과 같습니다.
5. BT의 특징
최근 몇 년 동안 BT는 계속 인기를 얻고 있지만, Finite State Machine(FSMs)도 여전히 로봇의 행동을 정의하거나 그 외 다른 소프트웨어에서도 가장 알 알려진 패러다임 중 하나입니다. FSM은 하나 혹은 그 이상의 state를 갖는 abstractive(virtual) machine입니다. 이 machine은 여러 작업을 수행하기 위해 여러 state의 상태를 바꾸는데, 이는 한 번에 하나의 state만 활성화할 수 있기 때문입니다.
알고리즘, 로보틱스, 게임 같은 것들을 만들 때, FSM은 execution flow를 만들기 위해 자주 사용됩니다. 예를 들어, 로봇의 "뇌" 역할을 하는 소프트웨어를 만들기 위해 사용될 수도 있습니다. 또 다른 예시로, ROS1의 FlexBe가 FSM입니다. 일반적으로 FSM은 좀 더 높은 추상적인 레벨에서의 로봇 어플리케이션을 만들 수 있도록 도와줍니다.
아래 그림은 FSM의 예시를 보여줍니다.
위 그림에서 각 node는 state를 의미하고, 화살표는 transition을 의미합니다. 각 화살표의 레이블은 언제 해당 transition이 일어나야 하는지를 나타냅니다.
5.1. FSM과 BT의 비교
실제 환경에서 state와 condition사이에 너무 많은 transition을 갖는 FMS을 사용하는 경우, 다음과 같은 이슈들이 있습니다.
- N개의 state를 갖는 FSM은 최대 N*N개의 transition이 있을 수 있습니다
- 이때, 뭔가 기능이 하나가 필요할 때마다 state의 개수에 영향을 미치므로... N 값이 빠르게 증가할 수 있습니다.
- 새로운 state가 추가되거나 기존의 state가 사라지면, transition된 모든 다른 state들의 conditions를 다 바꿔야 합니다.
- State들이 매우 타이트하게 연결되어 있다는 특징은 state를 재사용 하는데에 영향을 줍니다.
- 모델이 커지면 시각적으로나 코드상으로나... 굉장히 복잡해지게 됩니다.
FSM이 아무리 커져도 컴퓨터는 실행하는데 별 문제는 없지만, 정말 큰 FSM을 사람이 보면, 딱 보고 이게 뭐 하는 것인지 알아보는게 쉽지 않습니다.
이러한 문제들이 BT에서는 많이 해결됩니다. BT는 modularity와 재사용성이 강화되었는데, 이는 근본적으로 계층적 구조를 갖기 때문입니다.
- 어떠한 subtree이든지 재사용할 수 있는 가능성이 있습니다.
- BT에서 제공하는 언어를 활용하고 확장하여 대중적인 디자인 패턴을 적용할 수 있습니다.
- 계층 구조라는 것은 위에서 부터 아래로 내려오고, 왼쪽 node가 오른쪽 보다 우선시 된다는 것은 문맥적으로, 시각적으로 모두 사람이 이해하기 쉬운 형태입니다.
아래 그림은 같은 과정을 FSM으로 나타내는 경우와 BT로 나타내는 경우에 대한 예시입니다.
5.2. BT의 주요한 장점
이전에 언급했듯, BT가 갖고있는 장점들은 다음과 같습니다.
- 모듈화(Modular) : modular system은 각 요소들이 더 작은 요소로 쉽게 분해되고, 다시 합쳐지는 것이 쉽다는 것을 의미합니다. 따라서 이러한 구조의 장점 중 하나는, 디자인하고, 구현하고, 테스트하는데 있어서 분할정복(divide-and-conquer)을 적용할 수 있다는 것입니다. BT에서의 각각의 subtree는 모듈이라고 볼 수 있는데, 즉 가장 높은 곳의 subtree부터 leaves까지 모두 모듈화가 가능하다는 것입니다.
- 재사용성(Reusable code) : 재사용 가능한 코드는 크고 복잡한 대형 프로젝트에서 매우 중요합니다. BT의 모든 subtree는 BT의 여러 곳에서 재사용 가능합니다. 또한, leaf node의 코드를 작성할 때,
- 반응성(Reactivity) : 반응성이란, 제 시간에 효과적인 방식으로 변화에 대응할 수 있는 능력을 의미합니다. offline으로 만들어진 plan을 순차적으로 open loop으로 실행하면, 예기치 못한 상황에서 쉽게 Failure하는 경향이 있습니다. 이는 불확실하거나 예상치 못한 외부 요인들이 주변 환경을 계속 바꾸기 때문입니다. BT는 tick이 tree를 계속 돌아다니기 때문에 closed loop을 형성하게 되고, 이는 BT가 반응성을 갖도록 해줍니다.
- 사람이 이해하기 쉬움(Human readable) : 어쨌든간에 사람이 관리를 해줘야하기 때문에... 사람이 이해하기 쉬운 구조라는 것은 개발 비용을 줄이는 데 있어서 아주 큰 도움이 됩니다.