Skip to content

Subscription callback cannot work in my project #7

Description

@Noah-Lau

I write a Subscription callback of ros2 in my IsHomeCondition, but it didn't work. Though I send topic /localizaiton in terminal, it didn't output log, and other things, Could you please look my code, check what problem happend?

THIS IS MY CODE.

#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");
}

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

      Milestone

      No milestone

      Relationships

      None yet

      Development

      No branches or pull requests

      Issue actions