Publisher ZMQ - Server and Publisher Quitting

[mitdo]

I wanted to create a connection between Groot and the BehaviorTree.CPP library on ros2-foxy. I'm working in a docker container and installed all relevant packages. Firstly I installed the behaviortree-pkg via apt install and afterwards I tried installation from src. On the first try, the ZMQ publisher and server died. On the second try, only the publisher died. The error msg is shown below.

Dockerfile

## behavior tree
RUN apt-get update && \
    apt-get install -y \
    libzmq3-dev \
    libboost-dev \
    ros-$ROS2_DISTRO-behaviortree-cpp-v3

## GROOT
RUN apt-get update && \
    apt-get install -y \
    qtbase5-dev \
    libqt5svg5-dev \
    libzmq3-dev \
    libdw-dev

Groot installation

git clone https://github.com/BehaviorTree/Groot.git
colcon build
ros2 run groot Groot

ROS2 node

from: https://github.com/slaghuis/flight_control

with the added lines:

// set PublisherZMQ to connect the tree with GROOT
      RCLCPP_INFO(this->get_logger(), "set ZMQ Publisher");
      unsigned max_msg_per_second = 25;
      unsigned publisher_port = 1666;
      unsigned server_port = 1667;
      PublisherZMQ publisher_zmq(tree,max_msg_per_second, publisher_port, server_port);

// Copyright (c) 2021 Xeni Robotics
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/* **********************************************************************
 * Reads a behavior tree xml file and calls various action servers and 
 * simple services to complete the mission.
 * **********************************************************************/

#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <fstream>   // infile

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include "sensor_msgs/msg/battery_state.hpp"

#include "flight_control/utility_action.h"
#include "flight_control/drone_move_action.h"
#include "flight_control/drone_takeoff_action.h"
#include "flight_control/drone_land_action.h"
#include "flight_control/drone_target_land_action.h"
#include "flight_control/camera_save_action.h"
#include "flight_control/map_load_action.h"
#include "flight_control/map_save_action.h"
#include "flight_control/photogrammetry_action.h"
#include "flight_control/coverage_action.h"
#include "flight_control/camera_model_action.h"

#include "navigation_interfaces/srv/mission.hpp"

#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>

using Mission = navigation_interfaces::srv::Mission;
using namespace std::chrono_literals;
using namespace BT;

class FlightControlNode : public rclcpp::Node
{
  public:
    FlightControlNode()
    : Node("flight_control_node"), current_battery_voltage_(14.8), clear_to_fly_(false)
    {
      
      one_off_timer_ = this->create_wall_timer(
        1000ms, std::bind(&FlightControlNode::init, this));
        
    }

  private:
  
    void init ()
    {
      // Only run this once
      this->one_off_timer_->cancel();
      
      // Declare and get parameters
      behaviour_tree_file_ = this->declare_parameter<std::string>("mission_bt_file", "mission.xml");
      minimum_battery_voltage_ = this->declare_parameter<float>("minimum_battery_voltage", 13.6);
      clear_to_fly_ = ! this->declare_parameter<bool>("use_ground_control", false);
      drone_code_ = this->declare_parameter<int>("drone_code", 42);
      /*
      // Handle case where no ground control is to be used
      if (clear_to_fly_) {
        RCLCPP_INFO(this->get_logger(), "starting mission from main");
        read_mission_file();
      }*/

      // Setup the behavior tree
      using namespace DroneNodes;
      factory.registerSimpleCondition("BatteryOK", std::bind(&FlightControlNode::CheckBattery, this));
 
      factory.registerNodeType<DroneTakeoffAction>("TakeoffDrone");
      factory.registerNodeType<DroneLandAction>("LandDrone");
      factory.registerNodeType<DroneTargetLandAction>("TargetLandDrone");
      factory.registerNodeType<DroneMoveAction>("MoveDrone");
      factory.registerNodeType<SaySomething>("SaySomething");
      factory.registerNodeType<GenerateFilename>("GenerateFilename");
      factory.registerNodeType<CameraSaveAction>("SavePicture");
      factory.registerNodeType<MapLoadAction>("LoadMap");
      factory.registerNodeType<MapSaveAction>("SaveMap");
      factory.registerNodeType<PhotogrammetryAction>("Photogrammetry");
      factory.registerNodeType<CoverageAction>("CalculateCoverage");
      factory.registerNodeType<CameraModelAction>("CameraModel");
      
      // mission file creates behavior tree
      // this must be called after registration of NOdes
      // the BT cration is also necessary as timer_callback ticks the Tree
      // mission_start service has no function
      read_mission_file(); 

      subscription_ = this->create_subscription<sensor_msgs::msg::BatteryState>(
      "drone/battery", 5, std::bind(&FlightControlNode::battery_callback, this, std::placeholders::_1));
      
      mission_service_ = this->create_service<Mission>("drone/mission", 
        std::bind(&FlightControlNode::start_mission, this, std::placeholders::_1, std::placeholders::_2));

      timer_ = this->create_wall_timer(
        1000ms, std::bind(&FlightControlNode::timer_callback, this));

      // set PublisherZMQ to connect the tree with GROOT
      RCLCPP_INFO(this->get_logger(), "set ZMQ Publisher");
      unsigned max_msg_per_second = 25;
      unsigned publisher_port = 1666;
      unsigned server_port = 1667;
      PublisherZMQ publisher_zmq(tree,max_msg_per_second, publisher_port, server_port);

    }
    

    void start_mission(const std::shared_ptr<Mission::Request> request,
                          std::shared_ptr<Mission::Response> response)
    {
      if (request->drone_code == drone_code_) {
      
        behaviour_tree_file_ = request->mission_file;
        
        if (!is_file_exist(behaviour_tree_file_.c_str())) {
          RCLCPP_ERROR(this->get_logger(), "Mission file : '%s' not found.", behaviour_tree_file_.c_str());
          clear_to_fly_ = false;
        } else {
          clear_to_fly_ = read_mission_file();       
        }  
      } else {
        clear_to_fly_ = false;
      }
      response->accepted = clear_to_fly_;  
    }                      
    
    bool is_file_exist(const char *fileName)
    {
      std::ifstream infile(fileName);
      return infile.good();
    }
    
    bool read_mission_file() {
      if (!is_file_exist(behaviour_tree_file_.c_str())) {
        RCLCPP_ERROR(this->get_logger(), "Mission file : '%s' not found.", behaviour_tree_file_.c_str());
        return false;
      }
      RCLCPP_INFO(this->get_logger(), "Reading mission file : '%s'", behaviour_tree_file_.c_str());
      tree = factory.createTreeFromFile(behaviour_tree_file_);
      RCLCPP_INFO(this->get_logger(), "Successfully read mission file : '%s'", behaviour_tree_file_.c_str());
      
      

      auto node_ptr = shared_from_this();                
      // Iterate through all the nodes and call init() if it is an Action_B
      using namespace DroneNodes;
      for( auto& node: tree.nodes )
      {
        // Not a typo: it is "=", not "=="
        if( auto takeoff_action = dynamic_cast<DroneTakeoffAction*>( node.get() )) {
          takeoff_action->init( node_ptr );
        } else if( auto land_action = dynamic_cast<DroneLandAction*>( node.get() )) {
          land_action->init( node_ptr );
        } else if( auto target_land_action = dynamic_cast<DroneTargetLandAction*>( node.get() )) {
          target_land_action->init( node_ptr );
        } else if( auto move_action = dynamic_cast<DroneMoveAction*>( node.get() )) {
          move_action->init( node_ptr );
        } else if( auto save_picture_action = dynamic_cast<CameraSaveAction*>( node.get() )) {
          save_picture_action->init( node_ptr );
        } else if( auto load_action = dynamic_cast<MapLoadAction*>( node.get() )) {
          load_action->init( node_ptr );
        } else if( auto save_action = dynamic_cast<MapSaveAction*>( node.get() )) {
          save_action->init( node_ptr );
        } else if( auto save_action = dynamic_cast<PhotogrammetryAction*>( node.get() )) {
          save_action->init( node_ptr );
        } else if( auto save_action = dynamic_cast<CoverageAction*>( node.get() )) {
          save_action->init( node_ptr );
        } else if( auto mystery_action = dynamic_cast<CameraModelAction*>( node.get() )) {
          mystery_action->init( node_ptr );
        }
      }
      
      return true;
    }

    
    BT::NodeStatus CheckBattery()
    {      
      return (current_battery_voltage_ >= minimum_battery_voltage_) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
    }  
  
    void battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg) 
    {
      current_battery_voltage_ = msg->voltage;
    }
    rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr subscription_;
    
    void timer_callback()
    {      
      using namespace DroneNodes;
      
      if( !clear_to_fly_ ) {
        return;
      }

      
      clear_to_fly_ = ( tree.tickRoot() == NodeStatus::RUNNING);  // Stop flight once the tree is done.
    }
    
    BehaviorTreeFactory factory;
    Tree tree;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::TimerBase::SharedPtr one_off_timer_;
    float minimum_battery_voltage_;
    float current_battery_voltage_;
    int drone_code_;
    
    std::string behaviour_tree_file_;
    bool clear_to_fly_;
    rclcpp::Service<Mission>::SharedPtr mission_service_;    
};

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

1) installing by: apt install ros-foxy-behaviortree-cpp-v3

[PublisherZMQ] Server quitting.
[PublisherZMQ] just died. Exception Context was terminated
[PublisherZMQ] Publisher quitting.
[PublisherZMQ] just died. Exception Context was terminated

2) installing behaviortree-cpp-v3 from source

via

git clone && colcon build

and changing the ip from * to 0.0.0.0

sprintf(str, "tcp://0.0.0.0:%d", publisher_port);
    zmq_->publisher.bind(str);
    std::cout << str << std::endl;
    sprintf(str, "tcp://0.0.0.0:%d", server_port);
    zmq_->server.bind(str);
    std::cout << str << std::endl;

brings following error, only the publisher died

[flight_control_node-1] [INFO] [1652459928.909765611] [flight_control_node]: set ZMQ Publisher
[flight_control_node-1] tcp://0.0.0.0:1666
[flight_control_node-1] tcp://0.0.0.0:1667
[flight_control_node-1] [PublisherZMQ] Publisher quitting.
[flight_control_node-1] [PublisherZMQ] just died. Exception Context was terminated