behavior_node.h

Go to the documentation of this file.

/****************************************************************************
 *  Copyright (C) 2019 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/
 
#ifndef ROBORTS_DECISION_BEHAVIOR_NODE_H
#define ROBORTS_DECISION_BEHAVIOR_NODE_H
 
#include <chrono>
#include <thread>
#include <vector>
 
#include "../blackboard/blackboard_raw.h"
#include "behavior_state.h"
#include "../strategy/strategy_execute.h"
 
namespace roborts_decision{
enum class BehaviorType {
  PARALLEL,       
  SELECTOR,       
  SEQUENCE,       
  ACTION,         
  PRECONDITION,   
};
/*enum class AbortType {
  NONE,           ///<Do not abort anything
  SELF,           ///<Abort self, and any sub-trees running under this node
  LOW_PRIORITY,   ///<Abort any nodes to the right of this node
  BOTH            ///<Abort self, any sub-trees running under me, and any nodes to the right of this node
};*/
class BehaviorNode : public std::enable_shared_from_this<BehaviorNode>{
 public:
 
  typedef std::shared_ptr<BehaviorNode> Ptr;
  BehaviorNode(){}
  BehaviorNode(std::string name, BehaviorType behavior_type, const Blackboard::Ptr &blackboard_ptr):
      name_(name),
      behavior_type_(behavior_type),
      blackboard_ptr_(blackboard_ptr),
      behavior_state_(BehaviorState::IDLE),
      level_(0){}
 
  virtual ~BehaviorNode()= default;
  BehaviorState Run(){
 
    if (behavior_state_ != BehaviorState::RUNNING) {
      OnInitialize();
    }
 
    behavior_state_ = Update();
 
    if (behavior_state_ != BehaviorState::RUNNING) {
      OnTerminate(behavior_state_);
    }
 
    return behavior_state_;
  }
  virtual void Reset(){
    if (behavior_state_ == BehaviorState::RUNNING){
      behavior_state_ = BehaviorState::IDLE;
      OnTerminate(behavior_state_);
    }
  }
  BehaviorType GetBehaviorType(){
    return behavior_type_;
  }
  BehaviorState GetBehaviorState(){
    return behavior_state_;
  }
  std::string GetName(){
    return name_.c_str();
  }
  void SetParent(BehaviorNode::Ptr  parent_node_ptr){
    parent_node_ptr_ = parent_node_ptr;
  }
  BehaviorNode::Ptr GetParent(){
    return parent_node_ptr_ ;
  }
  virtual BehaviorNode::Ptr GetChild(){
    return nullptr;
  }
  unsigned int GetLevel(){
    return level_;
  }
  void SetLevel(unsigned int level){
    level_ = level;
  }
 protected:
  virtual BehaviorState Update() = 0;
  virtual void OnInitialize() = 0;
  virtual void OnTerminate(BehaviorState state) = 0;
 
  std::string name_;
//  std::mutex behavior_state_mutex_;
  BehaviorState behavior_state_;
  BehaviorType behavior_type_;
  Blackboard::Ptr blackboard_ptr_;
  BehaviorNode::Ptr parent_node_ptr_;
  unsigned  int level_;
};
 
class CompositeNode: public BehaviorNode{
 public:
  CompositeNode(std::string name, BehaviorType behavior_type, const Blackboard::Ptr &blackboard_ptr):
      BehaviorNode::BehaviorNode(name, behavior_type, blackboard_ptr),
      children_node_index_(0) {
  }
 
  virtual ~CompositeNode()= default;
  virtual void AddChildren(const BehaviorNode::Ptr& child_node_ptr){
    children_node_ptr_.push_back(child_node_ptr);
    child_node_ptr->SetParent(shared_from_this());
    child_node_ptr->SetLevel(level_+1);
  }
  virtual void AddChildren(std::initializer_list<BehaviorNode::Ptr> child_node_ptr_list){
    for (auto child_node_ptr = child_node_ptr_list.begin(); child_node_ptr!=child_node_ptr_list.end(); child_node_ptr++) {
      children_node_ptr_.push_back(*child_node_ptr);
      (*child_node_ptr)->SetParent(shared_from_this());
      (*child_node_ptr)->SetLevel(level_+1);
    }
  }
  virtual BehaviorNode::Ptr GetChild(){
    return children_node_ptr_.at(children_node_index_) ;
  }
  std::vector<BehaviorNode::Ptr>& GetChildren(){
    return children_node_ptr_;
  }
  unsigned int GetChildrenIndex(){
    return children_node_index_;
  }
  unsigned int GetChildrenNum(){
    return children_node_ptr_.size();
  }
 
 protected:
  virtual BehaviorState Update() = 0;
  virtual void OnInitialize() = 0;
  virtual void OnTerminate(BehaviorState state) = 0;
  std::vector<BehaviorNode::Ptr> children_node_ptr_;
  unsigned int children_node_index_;
 
};
class SelectorNode: public CompositeNode{
 public:
  SelectorNode(std::string name, const Blackboard::Ptr &blackboard_ptr):
      CompositeNode::CompositeNode(name, BehaviorType::SELECTOR, blackboard_ptr) {
    this->ptr_selector_node_ = nullptr;
  }
  virtual ~SelectorNode() = default;
 
 
   virtual void AddChildren(std::initializer_list<std::shared_ptr<AbstractCommonStrategy>> child_node_ptr_list);
 
 protected:
  void SetChildrenIndex(unsigned int children_node_index);
 
  virtual void OnInitialize(){
    children_node_index_ = 0;
    ROS_INFO("%s %s", name_.c_str(), __FUNCTION__);
  }
 
  virtual BehaviorState Update();
 
  std::shared_ptr<AbstractCommonStrategy> GetAction();
 
  bool HasActionOnGoing();
 
  void ExitThisAction();
 
  std::shared_ptr<AbstractCommonStrategy> ChooseActionToExecute();
 
  void OnTerminate(BehaviorState state);
 private:
  std::shared_ptr<AbstractCommonStrategy> ptr_selector_node_;
  std::vector<std::shared_ptr<AbstractCommonStrategy>> children_node_ptr_vector_;
 
 
};
 
 
 
class ParallelNode: public CompositeNode{
 public:
  ParallelNode(std::string name, const Blackboard::Ptr &blackboard_ptr,
               unsigned int threshold):
      CompositeNode::CompositeNode(name, BehaviorType::PARALLEL, blackboard_ptr),
      threshold_(threshold),
      success_count_(0),
      failure_count_(0){
 
  }
 
  virtual ~ParallelNode() = default;
 protected:
  virtual void OnInitialize(){
    failure_count_=0;
    success_count_=0;
    children_node_done_.clear();
    children_node_done_.resize(children_node_ptr_.size(),false);
    ROS_INFO("initialize start");
    ROS_INFO("%s %s", name_.c_str(), __FUNCTION__);
  }
  virtual BehaviorState Update(){
 
    if (children_node_ptr_.empty()) {
      return BehaviorState::SUCCESS;
    }
 
    for (unsigned int index = 0; index!=children_node_ptr_.size(); index++) {
      if (!children_node_done_.at(index)){
        BehaviorState state = children_node_ptr_.at(index)->Run();
 
/*        if (state == BehaviorState::SUCCESS) {
          children_node_done_.at(index) = true;
          if (++success_count_ >= threshold_) {
            return BehaviorState::SUCCESS;
          }
        }
        else if (state == BehaviorState::FAILURE) {
          children_node_done_.at(index) = true;
          if (++failure_count_ >= children_node_ptr_.size()-threshold_) {
            return BehaviorState::FAILURE;
          }
        }*/
 
      }
    }
    return BehaviorState::RUNNING;
  }
  virtual void OnTerminate(BehaviorState state){
    switch (state){
      case BehaviorState::IDLE:
        ROS_INFO("%s %s IDLE!", name_.c_str(), __FUNCTION__);
        break;
      case BehaviorState::SUCCESS:
        ROS_INFO("%s %s SUCCESS!", name_.c_str(), __FUNCTION__);
        break;
      case BehaviorState::FAILURE:
        ROS_INFO("%s %s FAILURE!", name_.c_str(), __FUNCTION__);
        break;
      default:
        ROS_INFO("%s %s ERROR!", name_.c_str(), __FUNCTION__);
        return;
    }
    //TODO: no matter what state, the node would reset all running children to terminate.
    for (unsigned int index = 0; index!=children_node_ptr_.size(); index++) {
      children_node_ptr_.at(index)->Reset();
    }
  }
 
  std::vector<bool> children_node_done_;
  unsigned int success_count_;
  unsigned int failure_count_;
  unsigned int threshold_;
};
 
 
} //namespace roborts_decision
 
 
#endif //ROBORTS_DECISION_BEHAVIOR_NODE_H