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