지난 프로젝트에서는 nav2에서 만들어진 node로 design만 하는 XML을 작성했다.

하지만 필요한 decorator/control/condition/action 노드가 미리 생성되어 있을 수 없으므로 개발을 위해 CPP로 node를 만들어야 한다.

지난 프로젝트에서도 "길을 잃었을 때만 카메라를 켜서 아루코마커 디텍션을 할 수 있나요?" 같은 질문이 있었는데, 당시에는 빠른 개발을 위해 Python으로만 개발을 하기로 했었기에 노드를 직접 만들지는 않았고, 여러 번 시도 후 길을 못 갈 때는 (주행을 맡은 다른 팀원이) BT를 탈출해서 cmd_vel을 publish하도록 했다.

아쉬움이 남았기 때문에 CPP로 node를 만들어 보기로 했고, 공식문서 외에 유튜브를 참고했다.

사용한 C++은 17, Behavior Tree.CPP는 4.5 버전이다.

BehaviorTree 설치

1. BehaviorTree.CPP git clone

git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git

2. build tool & dependency install

sudo apt-get install cmake libzmq3-dev libboost-dev libsqlite3-dev libgtest-dev

3. build

cd BehaviorTree.CPP
mkdir build ; cd build
cmake ..
make

*ROS의 catkin이나 ROS2의 colcon으로 빌드할 수도 있다.

4. install

sudo make install

BehaviorTree 코드 작성

• simple_bt.cpp

#include <iostream>
#include <chrono>
#include "behaviortree_cpp/action_node.h"
#include "behaviortree_cpp/bt_factory.h"

// Example of custom SyncActionNode (synchronous action)
// without ports.
class ApproachObject : public BT::SyncActionNode
{
public:
  ApproachObject(const std::string& name) :
      BT::SyncActionNode(name, {})
  {}

  // You must override the virtual function tick()
  BT::NodeStatus tick() override
  {
    std::cout << "ApproachObject: " << this->name() << std::endl;
    return BT::NodeStatus::SUCCESS;
  }
};

using namespace BT;

// Simple function that return a NodeStatus
BT::NodeStatus CheckBattery()
{
  std::cout << "[ Battery: OK ]" << std::endl;
  return BT::NodeStatus::SUCCESS;
}

// We want to wrap into an ActionNode the methods open() and close()
class GripperInterface
{
public:
  GripperInterface(): _open(true) {}
    
  NodeStatus open() 
  {
    _open = true;
    std::cout << "GripperInterface::open" << std::endl;
    return NodeStatus::SUCCESS;
  }

  NodeStatus close() 
  {
    std::cout << "GripperInterface::close" << std::endl;
    _open = false;
    return NodeStatus::SUCCESS;
  }

private:
  bool _open; // shared information
};

int main()
{
    // We use the BehaviorTreeFactory to register our custom nodes
  BehaviorTreeFactory factory;

  // The recommended way to create a Node is through inheritance.
  factory.registerNodeType<ApproachObject>("ApproachObject");

  // Registering a SimpleActionNode using a function pointer.
  // You can use C++11 lambdas or std::bind
  factory.registerSimpleCondition("CheckBattery", [&](TreeNode&) { return CheckBattery(); });

  //You can also create SimpleActionNodes using methods of a class
  GripperInterface gripper;
  factory.registerSimpleAction("OpenGripper", [&](TreeNode&){ return gripper.open(); } );
  factory.registerSimpleAction("CloseGripper", [&](TreeNode&){ return gripper.close(); } );

  // Trees are created at deployment-time (i.e. at run-time, but only 
  // once at the beginning). 
    
  // IMPORTANT: when the object "tree" goes out of scope, all the 
  // TreeNodes are destroyed
  // std::cout << std::filesystem::current_path() << std::endl;
  auto tree = factory.createTreeFromFile("../my_tree.xml");

  // 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;
}

• bt_tree.xml

 <root BTCPP_format="4" >
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <CheckBattery   name="check_battery"/>
            <OpenGripper    name="open_gripper"/>
            <ApproachObject name="approach_object"/>
            <CloseGripper   name="close_gripper"/>
        </Sequence>
     </BehaviorTree>
 </root>

• CMakeLists.txt 작성

cmake_minimum_required(VERSION 3.5)

project(simple_bt LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(behaviortree_cpp REQUIRED)

add_executable(${PROJECT_NAME} "simple_bt.cpp")
target_link_libraries(${PROJECT_NAME} BT::behaviortree_cpp)

• build

mkdir build ; cd build
cmake ..
make

• 실행

./simple_bt

• 실행결과
  

• 뱀발: 상대경로를 인식 못하는 경우, 절대 경로를 체크해 볼 것

std::cout << std::filesystem::current_path() << std::endl;

build 디렉토리에서 ./simple_bt를 실행하면 현재 디렉토리에는 xml 파일이 없으므로 에러가 발생한다. 그래서 상위 디렉토리에서 찾게끔 설정하거나 실행하는 디렉토리를 다른 곳으로 해서 해결했다.

• nav2 BT plugin node 참고: https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp