Skip to content

Subscription callback cannot work in my project #7

@Noah-Lau

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