沉默杀手HomeArchivesTagsCategories
行为树常用代码
|路径规划行为树|
Word count: 558|Reading time: 3 min
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
class Test : public BT::SyncActionNode {
public:
    /**
     * @param name node name
     * @param config Node parameters and configuration information
     */
    Test(const std::string& name, const BT::NodeConfiguration& config);
    // 构造函数里 setRegistrationID
    /**
     * @brief Define node input parameters
     * @return BT::PortsList parameter list
     */
    static BT::PortsList providedPorts() { return {}; }
private:
    // 实现 tick 函数
    virtual BT::NodeStatus tick() override;
};

factory.registerNodeType<Test>("TestAction");
factory.registerSimpleAction("SimpleAction", SimpleActionFunc);
1
2
3
class Pipeline : public BT::ControlNode 

class RateController : public BT::DecoratorNode

每次tickRoot()函数执行都会遍历整个树，对于asynActionNode，因为其本身有循环(在单独线程里)，所以循环没有结束时会返回RUNNING状态。不同的控制流节点对RUNNING的处理不一样。

装饰节点

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#include "behaviortree_cpp_v3/decorator_node.h"

class RateController : public BT::DecoratorNode {
 public:
  /**
   * @brief RateController constructor
   * @param name name of this node
   * @param conf config of this node
   */
  RateController(const std::string& name, const BT::NodeConfiguration& conf)
  		: BT::DecoratorNode(name, conf), first_time_(false), initialized_(false) {}

  void Initialize()
  {
  	  double hz = 1.0;
	  getInput("hz", hz);
	  period_ = 1.0 / hz;
	  initialized_ = true;
	  first_time_ = true;
	  start_ = std::chrono::high_resolution_clock::now();
  }
  /**
   * @brief Provided ports
   * @return BT::PortsList ports list
   */
  static BT::PortsList providedPorts() {
    return {BT::InputPort<double>("hz", 10.0, "Rate")};
  }

 private:
  /**
   * @brief Node Execution Logic
   * @return BT::NodeStatus node status
   */
  BT::NodeStatus tick()
  {
  	  if (!initialized_)
  		  Initialize();
  
	  auto now = std::chrono::high_resolution_clock::now();
	  auto elapsed = now - start_;
	  auto seconds =
	      std::chrono::duration_cast<std::chrono::duration<float>>(elapsed);
	  if (first_time_ || seconds.count() >= period_) {
	    first_time_ = false;
	    const BT::NodeStatus child_status = child_node_->executeTick();
	    start_ = std::chrono::high_resolution_clock::now();
	    switch (child_status) {
	      case BT::NodeStatus::RUNNING:
	        return BT::NodeStatus::RUNNING;
	      case BT::NodeStatus::SUCCESS:
	        return BT::NodeStatus::SUCCESS;
	      case BT::NodeStatus::FAILURE:
	      default:
	        return BT::NodeStatus::FAILURE;
	    }
	  }
	  return BT::NodeStatus::FAILURE;
}

  bool first_time_;   // first tiem execute child node
  bool initialized_;  // initialization flag
  double period_;     // execute child node period
  std::chrono::time_point<std::chrono::high_resolution_clock>  start_;  // start time of tick
};

port

portslist中没有的key是不能用getInput或getOutput来操作的。

  • 获取port的值

getInput("hz", hz);

  • 设置port的值
1
2
3
task_ = config.blackboard->get<Task>("cur_task");
setOutput<std::string>("type", task_.type_);
setOutput<std::string>("state", task_.state_);
  • 在xml文件中声明ports
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
<Action ID="GetPath">
    <input_port name="value_A"/>
</Action>


<Decorator ID="RateController">
    <input_port name="hz" type="double">Rate</input_port>
</Decorator>
<Condition ID="ReachGoalCondition">
    <input_port name="dis_tol" type="float"/>
</Condition>

<SubTree ID="DilemmaSetControlHandler">
    <input_port default="true" name="__shared_blackboard">If false (default), the Subtree has an isolated blackboard and needs port remapping</input_port>
</SubTree>