?一、BehaviorTree + Groot 在ros中的運用的參考資料

1、BehaviorTree.cpp開源庫

地址：github.com/BehiaviorTree/BehaviorTree.CPP

提供BehaviorTree框架，提供examples學習，但是相對比較零碎，需要對BehaviorTree有一個初步了解

2、BehaviorTree主要概念、API與教程

地址：BehaviorTree.CPP

對開源庫有一些講解，相對清晰

3、古月居小明工坊的 《ROS實驗 | 行為樹實現機器人智能》
網頁鏈接：ROS實驗 | 行為樹實現機器人智能 - 古月居

二、BehaviorTree + Groot 在ros中的運用基礎

1、運行過程

一個名為“tick”的信號被發送到樹的根部，并在樹中傳播，直到到達葉節點。

接收tick信號的樹節點執行其callback。此回調必須返回。

SUCCESS,FAILURE or RUNNING

如果一個TreeNode有一個或多個子節點，它將根據其狀態、外部參數或前一個同級節點的結果來順序tick他們。

LeafNode，即那些沒有任何子節點的TreeNode，是實際的命令，即行為樹與系統其余部分交互的地方。Actions節點是最常見的葉節點類型。

2、tick的運行原理

Sequence 處在運行中, tick 傳到 DetectObject, 返回SUCCESS，tick傳到GraspObject,返回SUCCESS，子節點全部完成，父節點Sequence狀態變成SUCCESS。

其余詳細資料請參考提供的參考資料

三、調試工具

1、groot

Groot是與BehaviorTree.CPP搭配使用的工具，分為Editor、Monitor、Log Replay 3種模式，具有行為樹編輯、狀態監控、歷史log回放等功能。

指南：Groot - Interacting with Behavior Trees — Navigation 2 1.0.0 documentation

2、StdCoutLogger

作用：在終端打印行為樹中的節點執行狀態變化。

代碼僅需在加載tree后添加StdCoutLogger類的1個實例（且只能有1個實例），運行效果如下：

StdCoutLogger logger_cout(tree);

四、實踐

??????? 理論的東西看起來容易，但離實踐還有一定距離，接下來用一個實踐小項目來對BehaviorTree + Groot + Ros進行演示，項目題目來源于古月居，實現一個巡邏的小烏龜的游戲

Groot下的行為樹如下圖

運行過程

1、attack子節點haveEnemy判斷周圍是否有敵人。

2、若沒有敵人，Ation節點moveToEnemy將不被觸發，守衛將執行patrol節點，對區域進行搜索。

3、若找到敵人，moveToEnemy節點將被觸發，守衛向敵人前進。

1、小烏龜生成與tf發布

此部分不再詳細描述,代碼如下

void poseCallback(const turtlesim::PoseConstPtr& msg) {// tf廣播器static tf::TransformBroadcaster br;// 根據烏龜當前的位姿，設置相對于世界坐標系的坐標變換tf::Transform transform;transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );tf::Quaternion q;q.setRPY(0, 0, msg->theta);transform.setRotation(q);// 發布坐標變換br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); }int main(int argc, char** argv) {// 初始化節點ros::init(argc, argv, "my_tf_broadcaster");if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};turtle_name = argv[1];// 訂閱烏龜的pose信息ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);ros::spin();return 0; }; void poseCallback(const turtlesim::PoseConstPtr& msg) {// tf廣播器static tf::TransformBroadcaster br;// 根據烏龜當前的位姿，設置相對于世界坐標系的坐標變換tf::Transform transform;transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );tf::Quaternion q;q.setRPY(0, 0, msg->theta);transform.setRotation(q);// 發布坐標變換br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));// 發布固定點坐標tf::Transform transform_a;transform_a.setOrigin(tf::Vector3(1.0, 6.0, 0.0));tf::Quaternion q_a;q_a.setRPY(0.0, 0.0, 0.0);transform_a.setRotation(q_a);br.sendTransform(tf::StampedTransform(transform_a, ros::Time::now(), "world", "point_a"));tf::Transform transform_b;transform_b.setOrigin(tf::Vector3(10.0, 6.0, 0.0));tf::Quaternion q_b;q_b.setRPY(0.0, 0.0, 0.0);transform_b.setRotation(q_b);br.sendTransform(tf::StampedTransform(transform_b, ros::Time::now(), "world", "point_b")); }int main(int argc, char** argv) {// 初始化節點ros::init(argc, argv, "my_tf_broadcaster");if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};turtle_name = argv[1];// 訂閱烏龜的pose信息ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);// 通過服務調用，產生第二只烏龜turtle2ros::service::waitForService("spawn");ros::ServiceClient add_turtle =node.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawn srv;add_turtle.call(srv);ros::spin();return 0; };

2、Action節點構建

??????? 節點構建以靜態庫的方式進行構建，構建的類需要對behaviorTree的類進行繼承，巡邏節點類構建如下

class BTActionPatrol : public BT::AsyncActionNode { protected:ros::NodeHandle nh_;// 定義turtle2的速度控制發布器ros::Publisher turtle_vel =nh_.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);public:BTActionPatrol(const std::string& name, const BT::NodeConfiguration& config):BT:: AsyncActionNode(name, config){}~BTActionPatrol(void){}BT::NodeStatus tick() override;static BT::PortsList providedPorts(){return{ BT::InputPort<std::string>("message") };}virtual void halt() override; };

publisher與providedPorts定義本不應該放頭文件里面。

在構建節點時需要注意重寫 tick 與 halt函數，需要提供static BT::PortsList providedPorts接口函數

重寫的tick函數如下，主要操作在這里實現。

BT::NodeStatus BTActionPatrol::tick() {tf::TransformListener listener;tf::StampedTransform transform_a, transform_b, transform;// 查找坐標變換listener.waitForTransform("/turtle2", "/point_a", ros::Time(0), ros::Duration(3.0));listener.lookupTransform("/turtle2", "/point_a", ros::Time(0), transform_a);listener.waitForTransform("/turtle2", "/point_b", ros::Time(0), ros::Duration(3.0));listener.lookupTransform("/turtle2", "/point_b", ros::Time(0), transform_b);double distance_a, distance_b;distance_a = sqrt(pow(transform_a.getOrigin().x(), 2) + pow(transform_a.getOrigin().y(), 2));distance_b = sqrt(pow(transform_b.getOrigin().x(), 2) + pow(transform_b.getOrigin().y(), 2));if (nh_.hasParam("/goal_point")){if(distance_a < 0.5) {nh_.setParam("/goal_point", "b");ROS_INFO("Change direction to b");}else if(distance_b < 0.5){nh_.setParam("/goal_point", "a");ROS_INFO("Change direction to b");}}else{nh_.setParam("/goal_point", "a");}std::string direction;nh_.getParam("/goal_point", direction);if(direction == "a"){ROS_INFO("Nav to a");transform = transform_a;}else if(direction == "b"){ROS_INFO("Nav to b");transform = transform_b;}// 根據turtle1和turtle2之間的坐標變換，計算turtle2需要運動的線速度和角速度// 并發布速度控制指令，使turtle2向turtle1移動geometry_msgs::Twist vel_msg;vel_msg.angular.z = 1.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x()); // vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + // pow(transform.getOrigin().y(), 2));vel_msg.linear.x = 0.8;turtle_vel.publish(vel_msg);ROS_INFO("Action nav_b is successful!");return BT::NodeStatus::SUCCESS;}

moveToEnemy節點與上一節點一樣，繼承Action類，判斷是否有enemy的節點為條件節點，繼承Condition類，詳細代碼將不在此處詳寫。

3、構建樹

使用xml文件構建行為樹，代碼如下

<root main_tree_to_execute = "DoorClosed"><BehaviorTree ID="DoorClosed"><Fallback name="playGame"><Sequence name="attack"><haveEnemy/><moveToEnemy/></Sequence><patrol/> </Fallback></BehaviorTree> </root>

載入節點

BehaviorTreeFactory factory;factory.registerNodeType<BTActionNav>("moveToEnemy"); factory.registerNodeType<BTActionPatrol>("patrol"); factory.registerNodeType<BTActionHaveEnemy>("haveEnemy");auto tree = factory.createTreeFromText(xml_text);

添加Groot調試

PublisherZMQ publisher_zmq(tree);

4、修改CmakeLists

構建靜態庫

add_library(sample_nodes STATICsrc/treeNode/action_nav_enemy.cppsrc/treeNode/action_patrol.cppsrc/treeNode/condition_have_enemy.cpp)target_link_libraries(sample_nodes ${catkin_LIBRARIES} BT::behaviortree_cpp_v3)

?添加可執行文件

add_executable(guard_robot_tree src/tree/guard_robot_tree.cpp) target_link_libraries(guard_robot_tree${catkin_LIBRARIES}BT::behaviortree_cpp_v3sample_nodes# /opt/ros/melodic/lib/librosconsole.so# /opt/ros/melodic/lib/libroscpp_serialization.so )

5、運行效果

五、總結

對BehaviorTree進行了學習，還有很多功能有待開發，如節點間的通訊，由可以使用ros話題通訊代替，暫時不需要，后續有新內容還會進行更新，對于文章中的錯誤希望大家一起討論。

六、參考

BT9：各種調試工具介紹_linxigjs的博客-CSDN博客

ROS實驗 | 行為樹實現機器人智能 - 古月居

Behavior-tree 在ROS中的應用（入門）_bug有點多的博客-CSDN博客

總結

以上是生活随笔為你收集整理的BehaviorTree + Groot 在ros中的运用的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。