adlink-ros / bt_ros2 Goto Github PK

#include <string>
#include "somo_behavior_tree/plugins/condition/is_home_condition.hpp"

namespace behavior_tree
{
    IsHomeCondition::IsHomeCondition(
        const std::string &condition_name,
        const BT::NodeConfiguration &conf)
        : BT::ConditionNode(condition_name, conf),
        is_home_(false),
        localization_topic_("/localization")
        {
          node_ = node_ = rclcpp::Node::make_shared("IsHomeCondition");
          // node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
          localization_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
            localization_topic_,
            rclcpp::SystemDefaultsQoS(),
            std::bind(&IsHomeCondition::localization_cb, this, std::placeholders::_1));
          //parameters declare
          std::vector<std::double_t> home_position={0,0,0};
          node_->declare_parameter<std::vector<std::double_t>>("home_position",home_position);
          node_->declare_parameter<std::double_t>("check_threshold", 0.5);
          check_threshold_ = node_->get_parameter("check_threshold").as_double();

          home_position_.position.x = node_->get_parameter("home_position").as_double_array()[0];
          home_position_.position.y = node_->get_parameter("home_position").as_double_array()[1];
          std::cout << "Init completed" << std::endl;

        }
    IsHomeCondition::~IsHomeCondition()
    {
        RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsHomeCondition BT node");

    }



    BT::NodeStatus IsHomeCondition::tick()
    {
      std::cout << "home flag is" <<is_home_ << std::endl;
      if (is_home_) {
      std::cout << "SOMO is in not home"<< std::endl;
        return BT::NodeStatus::SUCCESS;

      }   
      std::cout << "SOMO is in not home"<< std::endl;
      return BT::NodeStatus::FAILURE; 
    }


    void IsHomeCondition::localization_cb(geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
      std::cout << "In localization callback" << std::endl;
      current_position_ = msg->pose;

        is_home_ = isHome();

    }

    bool IsHomeCondition::isHome(){
      return utils->check_distance(current_position_,home_position_,check_threshold_);

    }
        
}


#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<behavior_tree::IsHomeCondition>("IsHomeCondition");
}