Unable to use nav2 Behavior Tree Condition Node

edit

I tried to include nav2 behavior tree to check whether battery is low.

Here is my main code:

#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/utils/shared_library.h"

#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
#include "nav2_behavior_tree/behavior_tree_engine.hpp"


#define My_BT_XML "/home/robot/bt_ws/src/bt_test_pkg/bt_xml/example_bt.xml"

/* main class where you will initialize the behavior tree */
int main(int argc, char **argv)
{

  /* Initialize your tree */
  /* Pass the initialized tree to the ZMQ publisher */
  /* By adding following line you will be able to
     create a link between Groot and ROS */

  rclcpp::init(argc,argv);
  auto nh = rclcpp::Node::make_shared("BT_test");
  nh->declare_parameter("bt_xml",rclcpp::ParameterValue(std::string(My_BT_XML)));
  std::string bt_xml;
  nh->get_parameter("bt_xml",bt_xml);

  BT::BehaviorTreeFactory factory;
  BT::SharedLibrary loader;


  factory.registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>("IsBatteryLow");


  //Tree is destoried when it goes out of scope

  //factory.registerFromPlugin(loader.getOSName("nav2_is_battery_low_condition_bt_node"));
  auto tree = factory.createTreeFromFile(bt_xml);

  BT::NodeStatus status = BT::NodeStatus::RUNNING;
  while (rclcpp::ok() && status == BT::NodeStatus::RUNNING) {
        status = tree.tickRoot();
        // Sleep 100 milliseconds
        std::cout<<status<<"\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }


  BT::PublisherZMQ publisher_zmq(tree);

  return 0;
}

I have written a fake battery level publisher:

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/sensor_msgs/msg/battery_state.hpp"

using namespace std::chrono_literals;


class MinimalPublisher : public rclcpp::Node
{
  public:
    MinimalPublisher()
    : Node("battery_pub"), count_(0)
    {
      publisher_ = this->create_publisher<sensor_msgs::msg::BatteryState>("/battery_status",10);
      timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }

  private:
    void timer_callback()
    {
      auto batteryState_ = std::make_shared<sensor_msgs::msg::BatteryState>();
      batteryState_->percentage = 0.7;
      batteryState_->voltage = 8.5;
      publisher_->publish(*batteryState_);
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr publisher_;
    size_t count_;
};



int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}

The behavior tree is simply

<BehaviorTree ID="BehaviorTree">
        <Sequence>
            <Condition ID="IsBatteryLow" battery_topic="/battery_status" is_voltage="false" min_battery="0.5"/>
            <AlwaysSuccess/>
        </Sequence>
    </BehaviorTree>

However, when I run the publisher and BT tree code, I got the following error

terminate called after throwing an instance of 'BT::RuntimeError'
  what():  Blackboard::get() error. Missing key [node]
[ros2run]: Aborted