How to write a BT node that subscribe a topic (navigation2).

edit

I want to write a BT node that recieve state of a bumper.

But my code can not catch messages of a bumper.

How can I write a BT Node that works properly with subscription?

Thank you!

#include <iostream>
#include <memory>
#include <string>

#include <rclcpp/qos.hpp>
#include <rclcpp/rclcpp.hpp>

#include "behaviortree_cpp_v3/condition_node.h"
#include "gazebo_msgs/msg/contacts_state.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

namespace nav2_behavior_tree {

class BumperCollisionCondition : public BT::ConditionNode {
  public:
    BumperCollisionCondition(const std::string &condition_name, const BT::NodeConfiguration &conf);

    BumperCollisionCondition() = delete;

    BT::NodeStatus tick() override;

    static BT::PortsList providedPorts() {
        return {};
    }

  private:
    rclcpp::Subscription<gazebo_msgs::msg::ContactsState>::SharedPtr contact_state_;
    rclcpp::Node::SharedPtr node_;
    rclcpp::Time lasttime_;
};

} // namespace nav2_behavior_tree

namespace nav2_behavior_tree {

BumperCollisionCondition::BumperCollisionCondition(
    const std::string &condition_name,
    const BT::NodeConfiguration &conf)
    : BT::ConditionNode(condition_name, conf) {
  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
  auto qos = rclcpp::SystemDefaultsQoS();
  qos.best_effort();
  contact_state_ = node_->create_subscription<gazebo_msgs::msg::ContactsState>(
    "bumper_front",
    qos,
    [this](const gazebo_msgs::msg::ContactsState::SharedPtr msg) -> void {
      RCLCPP_INFO(node_->get_logger(), "TEST"); // "TEST" never print.
      auto states = msg->states;
      lasttime_ = rclcpp::Time(msg->header.stamp);
    });
}

BT::NodeStatus BumperCollisionCondition::tick() {
    RCLCPP_INFO(node_->get_logger(), "test %d", lasttime_.nanoseconds()); // lasttime_ never update.
    return BT::NodeStatus::FAILURE;
}

} // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
    factory.registerNodeType<nav2_behavior_tree::BumperCollisionCondition>("BumperCollision");
}