Behavior tree 編程實(shí)戰(zhàn)

• • 寫在前面的話
• 如何建立一棵行為樹
• • 行為樹枝干的編輯
  • 行為樹葉子結(jié)點(diǎn)的編輯

寫在前面的話

本文主要在前文的基礎(chǔ)上，通過《Behavior trees in ROS and AI》以及小明工坊中所提供的部分代碼進(jìn)行實(shí)戰(zhàn)化分析。
此處給出兩處代碼地址以及原文：
古月居小明工坊：https://www.guyuehome.com/5311
Behavior trees in ROS and AI:
https://behaviortree.github.io/BehaviorTree.CPP/tutorial_01_first_tree/

如何建立一棵行為樹

行為樹枝干的編輯

建立行為樹的方法主要有兩種，一種是靜態(tài)編譯，一種是XML動(dòng)態(tài)生成，這里主要剖析的是靜態(tài)編譯的方法。
建立一棵行為樹的最基本的架構(gòu)在
ROS-BehaviorTree/behavior_tree_core/src/gtest/external_ros_nodes_test.cpp
中得到了詳細(xì)的體現(xiàn)：

#include <behavior_tree.h>int main(int argc, char **argv) {ros::init(argc, argv, "BehaviorTree");try{int TickPeriod_milliseconds = 1000;BT::ROSAction* action = new BT::ROSAction("action");BT::NegationNode* decorator = new BT::NegationNode("decorator");BT::ROSCondition* condition = new BT::ROSCondition("condition");BT:: SequenceNode* sequence1 = new BT::SequenceNode("seq1");sequence1->AddChild(decorator);decorator->AddChild(condition);sequence1->AddChild(action);Execute(sequence1, TickPeriod_milliseconds); // from BehaviorTree.cpp}catch (BT::BehaviorTreeException& Exception){std::cout << Exception.what() << std::endl;}return 0; }

代碼心得：
behavior_tree.h 文件為mit所開發(fā)的一款關(guān)于行為樹創(chuàng)建的庫(kù)，可以通過以下的鏈接獲得：
https://github.com/BehaviorTree/BehaviorTree.CPP
整個(gè)枝干的創(chuàng)建過程基本上與C++中基礎(chǔ)的二叉樹的創(chuàng)建類似，通過new來生成新的結(jié)點(diǎn)以及定義指針來完成結(jié)點(diǎn)間對(duì)應(yīng)關(guān)系的構(gòu)建。但不同的是行為樹結(jié)合了ROS的特點(diǎn)，可以通過結(jié)點(diǎn)的調(diào)用將葉子進(jìn)行單獨(dú)的編寫，從而起到了模塊化的作用。
如果代碼正確而所觀察到的rqt_graph產(chǎn)生了錯(cuò)誤，即發(fā)生了節(jié)點(diǎn)間相互作用失敗的案例，可以嘗試通過檢查Cmake文件得以解決。
并在此處推薦ROS編譯的常用軟件Roboware，可以自動(dòng)生成Cmake.list以及pakeage.xml文件，還是比較方便的。
在創(chuàng)建完行為樹的基本枝干后就進(jìn)入到了行為樹葉子結(jié)點(diǎn)的編輯中。

行為樹葉子結(jié)點(diǎn)的編輯

ROS-BehaviorTree功能包中并沒有對(duì)葉子結(jié)點(diǎn)進(jìn)行具體功能的實(shí)現(xiàn)，只是在ROS-BehaviorTree/behavior_tree_core/src/路徑下給出了一些葉子結(jié)點(diǎn)編譯的模板程序。
而在古月居小明工坊的《ROS實(shí)驗(yàn) | 行為樹實(shí)現(xiàn)機(jī)器人智能》一文中給出了關(guān)于行為樹真正實(shí)現(xiàn)其功能的具體的案例。
我們以ROS-Behavior-Tree/behavior_tree_leaves/nodes/action_patrol.cpp為例，現(xiàn)在把代碼貼在下面：

#include <ros/ros.h> #include <actionlib/server/simple_action_server.h> #include <behavior_tree_core/BTAction.h> #include <string> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> #include <turtlesim/Spawn.h>enum Status {RUNNING, SUCCESS, FAILURE }; class BTAction { protected:ros::NodeHandle nh_;actionlib::SimpleActionServer<behavior_tree_core::BTAction> as_;std::string action_name_;//定義發(fā)布的反饋和結(jié)果behavior_tree_core::BTFeedback feedback_; //action 反饋，SUCCESS,FAILUREbehavior_tree_core::BTResult result_; ros::Publisher turtle_vel = nh_.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);public: //構(gòu)造函數(shù)，boost參數(shù)綁定explicit BTAction(std::string name) :as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),action_name_(name){as_.start();}~BTAction(){}void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal){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;}geometry_msgs::Twist vel_msg;vel_msg.angular.z = 1.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.linear.x = 0.8;turtle_vel.publish(vel_msg);set_status(SUCCESS);ROS_INFO("Action nav_b is successful!");}void set_status(int status){feedback_.status = status;result_.status = feedback_.status;as_.publishFeedback(feedback_);as_.setSucceeded(result_);switch (status) {case SUCCESS:ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() );break;case FAILURE:ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() );break;default:break;}} };int main(int argc, char** argv) {ros::init(argc, argv, "action");ROS_INFO(" Enum: %d", RUNNING);ROS_INFO(" Action Ready for Ticks");BTAction bt_action(ros::this_node::getName());ros::spin();return 0; }

這段代碼主要是在小海龜范圍內(nèi)沒有敵情時(shí)，在固定的兩個(gè)點(diǎn)間以擬好的速度進(jìn)行巡邏。
比較重要的是首先用了enum枚舉出了行為樹的三種反饋模式:
SUCCESS,FAILURE,RUNNING.
接下來使用了C++中較為方便的類以及析構(gòu)函數(shù)，將參數(shù)較為方便的傳遞給內(nèi)部函數(shù)以及變量，有助于實(shí)現(xiàn)程序的模塊化進(jìn)行。
接下來比較重要的就是這兩行：

behavior_tree_core::BTFeedback feedback_; //action 反饋，SUCCESS,FAILUREbehavior_tree_core::BTResult result_;

這兩行代碼主要是定義了行為樹所特有的反饋，以及結(jié)果（對(duì)于其實(shí)現(xiàn)的功能可以理解為結(jié)果，但實(shí)際上并不是結(jié)果那么簡(jiǎn)單）。
以及對(duì)于反饋與結(jié)果具體內(nèi)容的實(shí)現(xiàn)，可以在函數(shù) set_status中具體體現(xiàn)：

void set_status(int status){feedback_.status = status;result_.status = feedback_.status;as_.publishFeedback(feedback_);as_.setSucceeded(result_);switch (status) {case SUCCESS:ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() );break;case FAILURE:ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() );break;default:break;}} };

行為樹就是想這子葉反饋SUCCESS，FAILURE,或者RUNNING，將子葉的工作狀況反饋給上一級(jí)，從而實(shí)現(xiàn)不同情況的判斷，動(dòng)作執(zhí)行的時(shí)機(jī)等，使其具有了一定的智能。
在回調(diào)這一塊，我們?cè)?br /> ROS-Behavior-Tree/behavior_tree_leaves/nodes/condition_have_enemy.cpp
文件中可以得到更具體的展示：

distance = sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));nh_.setParam("/produc_status",distance);if(distance<2){ROS_INFO("Find enemy!");set_status(SUCCESS);}else{ROS_INFO("Everything is ok");set_status(FAILURE);}}

在這里我們可以更加清晰的看到SUCCESS與FAILURE在行為樹狀態(tài)結(jié)點(diǎn)實(shí)現(xiàn)其功能時(shí)的重要性，從而可以一窺行為樹實(shí)現(xiàn)其功能中tick()，即回調(diào)，的重要性。

總結(jié)

以上是生活随笔為你收集整理的Behavior tree 编程实战的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。