ros2_behavior_tree_example/src/plugins/log_status_bt_node.cpp at 1f5d948fdda2df5f5f5f64a6d1fcba80a64732b5 · polymathrobotics/ros2_behavior_tree_example · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#include "ros2_behavior_tree_example/plugins/log_status_bt_node.hpp"

namespace polymath
{
namespace bt_ros_example
{
    LogStatusNode::LogStatusNode(const std::string & action_name, const BT::NodeConfiguration & conf)
    : BT::SyncActionNode(action_name, conf),
      print_ping_pong_(false)
    {
        node_ = conf.blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
        auto msg = getInput<std::string>("message");
        if(!msg)
        {
           throw BT::RuntimeError( "missing required input [message]: ", msg.error() );
        }

        auto print_ping_pong = getInput<bool>("print_ping_pong");
        if(print_ping_pong)
        {
            print_ping_pong_ = print_ping_pong.value();
        }

        message_ = msg.value();
    }

    LogStatusNode::~LogStatusNode()
    {
        RCLCPP_INFO(node_->get_logger(),"SHUTTING DOWN PUBLISH STATUS NODE");
    }

    BT::NodeStatus LogStatusNode::tick()
    {
        RCLCPP_INFO(node_->get_logger(), message_.c_str());

        if(print_ping_pong_)
        {
            int32_t ping_id = -1;
            int32_t pong_id = -1;
            getInput<int32_t>("ping_id", ping_id);
            getInput<int32_t>("pong_id", pong_id);
            RCLCPP_INFO(node_->get_logger(), "sent ping: %d, received pong: %d", ping_id, pong_id);
        }

        return BT::NodeStatus::SUCCESS;
    }
}
}