Upgrade to Behaviortree.CPP v4

plansys2_bt_actions/CMakeLists.txt

@@ -9,39 +9,30 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(plansys2_executor REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 find_package(action_msgs REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 
-find_package(ZMQ)
-if(ZMQ_FOUND)
-    message(STATUS "ZeroMQ found.")
-    add_definitions(-DZMQ_FOUND)
-else()
-  message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.")
-endif()
-
 set(CMAKE_CXX_STANDARD 17)
 
 set(dependencies
     rclcpp
     rclcpp_action
     rclcpp_lifecycle
     plansys2_executor
-    behaviortree_cpp_v3
+    behaviortree_cpp
     action_msgs
     lifecycle_msgs
 )
 
-include_directories(include ${ZMQ_INCLUDE_DIRS})
+include_directories(include)
 
 set(BT_ACTIONS_SOURCES
   src/plansys2_bt_actions/BTAction.cpp
 )
 
 add_library(${PROJECT_NAME} SHARED ${BT_ACTIONS_SOURCES})
 ament_target_dependencies(${PROJECT_NAME} ${dependencies})
-target_link_libraries(${PROJECT_NAME} ${ZMQ_LIBRARIES})
 
 add_executable(bt_action_node
   src/bt_action_node.cpp
@@ -77,5 +68,6 @@ endif()
 
 ament_export_include_directories(include)
 ament_export_dependencies(${dependencies})
+include_directories(include)
 
 ament_package()

plansys2_bt_actions/README.md

@@ -20,7 +20,7 @@ Files created by the `.fbl` and minitrace loggers are stored in `/tmp/<node_name
 ### BT node for calling ROS2 action servers
 The `BtActionNode` template class provides a convenient means of calling ROS2 action servers from within a BT.  It takes care of the details of setting up and handling a ROS action client, reducing code duplication and providing a simple API.
 
-The template parameter for the class is the type of ROS action (e.g. `action_tutorials_interfaces::action::Fibonacci`) to be used. The node's constructor takes in three arguments: the XML tag name, the ROS topic for the action server (e.g. `/namepace/server_name`), and a `BT::NodeConfiguration`.  Note that the XML name and `NodeConfiguration` are the same as any other BT.CPP node.
+The template parameter for the class is the type of ROS action (e.g. `action_tutorials_interfaces::action::Fibonacci`) to be used. The node's constructor takes in three arguments: the XML tag name, the ROS topic for the action server (e.g. `/namepace/server_name`), and a `BT::NodeConfig`.  Note that the XML name and `NodeConfig` are the same as any other BT.CPP node.
 
 There are several functions which are provided for the end user to use/implement (some of which are optional).
 1. `static BT::PortsList providedPorts()`: every BT node which uses ports must define this member function.  A default implementation is provided, but you are free to override it if additional ports are desired.  By default, the function returns two input ports: `server_name` (string) and `server_timeout` (double).  These ports can be preserved when overriding using `providedBasicPorts`

plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp

@@ -19,15 +19,11 @@
 #include <string>
 #include <vector>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "behaviortree_cpp_v3/xml_parsing.h"
-#include "behaviortree_cpp_v3/loggers/bt_file_logger.h"
-#include "behaviortree_cpp_v3/loggers/bt_minitrace_logger.h"
-
-#ifdef ZMQ_FOUND
-#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>
-#endif
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/xml_parsing.h"
+#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
+#include "behaviortree_cpp/loggers/bt_minitrace_logger.h"
 
 #include "plansys2_executor/ActionExecutorClient.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -69,8 +65,7 @@ class BTAction : public plansys2::ActionExecutorClient
   std::string bt_xml_file_;
   std::vector<std::string> plugin_list_;
   bool finished_;
-  std::unique_ptr<BT::PublisherZMQ> publisher_zmq_;
-  std::unique_ptr<BT::FileLogger> bt_file_logger_;
+  std::unique_ptr<BT::FileLogger2> bt_file_logger_;
   std::unique_ptr<BT::MinitraceLogger> bt_minitrace_logger_;
 };
 

plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp

@@ -18,7 +18,7 @@
 #include <memory>
 #include <string>
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
@@ -35,10 +35,12 @@ class BtActionNode : public BT::ActionNodeBase
   BtActionNode(
     const std::string & xml_tag_name,
     const std::string & action_name,
-    const BT::NodeConfiguration & conf)
+    const BT::NodeConfig & conf)
   : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
   {
-    config().blackboard->get("node", node_);
+    if (!config().blackboard->get("node", node_)) {
+      RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard");
+    }
 
     // Get the required items from the blackboard
     server_timeout_ = 5s;
@@ -95,6 +97,7 @@ class BtActionNode : public BT::ActionNodeBase
         5.0,
         "The amount of time to wait for a response from the action server, in seconds")
     };
+    // The user defined ports are added to the basic ports
     basic.insert(addition.begin(), addition.end());
 
     return basic;
@@ -307,7 +310,7 @@ class BtActionNode : public BT::ActionNodeBase
       cancel_goal();
     }
 
-    setStatus(BT::NodeStatus::IDLE);
+    resetStatus();
   }
 
 protected:

plansys2_bt_actions/package.xml

@@ -16,7 +16,7 @@
   <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>plansys2_executor</depend>
-  <depend>behaviortree_cpp_v3</depend>
+  <depend>behaviortree_cpp</depend>
   <depend>action_msgs</depend>
   <depend>libzmq3-dev</depend>
 

plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp

@@ -22,7 +22,7 @@
 #include <memory>
 #include <chrono>
 
-#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp/utils/shared_library.h"
 #include "plansys2_bt_actions/BTAction.hpp"
 
 namespace plansys2
@@ -38,12 +38,6 @@ BTAction::BTAction(
     "plugins", std::vector<std::string>({}));
   declare_parameter<bool>("bt_file_logging", false);
   declare_parameter<bool>("bt_minitrace_logging", false);
-#ifdef ZMQ_FOUND
-  declare_parameter<bool>("enable_groot_monitoring", true);
-  declare_parameter<int>("publisher_port", -1);
-  declare_parameter<int>("server_port", -1);
-  declare_parameter<int>("max_msgs_per_second", 25);
-#endif
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
@@ -75,7 +69,6 @@ BTAction::on_configure(const rclcpp_lifecycle::State & previous_state)
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 BTAction::on_cleanup(const rclcpp_lifecycle::State & previous_state)
 {
-  publisher_zmq_.reset();
   return ActionExecutorClient::on_cleanup(previous_state);
 }
 
@@ -93,8 +86,12 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
   }
 
   for (int i = 0; i < get_arguments().size(); i++) {
+    auto arg = get_arguments()[i];
+    RCLCPP_DEBUG_STREAM(
+      get_logger(),
+      "Setting arg" << i << " [" << arg << "]");
     std::string argname = "arg" + std::to_string(i);
-    blackboard_->set(argname, get_arguments()[i]);
+    blackboard_->set(argname, arg);
   }
 
   if (get_parameter("bt_file_logging").as_bool() ||
@@ -116,7 +113,7 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
         get_logger(),
         "Logging to file: " << filename_extension);
       bt_file_logger_ =
-        std::make_unique<BT::FileLogger>(tree_, filename_extension.c_str());
+        std::make_unique<BT::FileLogger2>(tree_, filename_extension.c_str());
     }
 
     if (get_parameter("bt_minitrace_logging").as_bool()) {
@@ -129,44 +126,13 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
     }
   }
 
-#ifdef ZMQ_FOUND
-  bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool();
-  int publisher_port = get_parameter("publisher_port").as_int();
-  int server_port = get_parameter("server_port").as_int();
-  unsigned int max_msgs_per_second = get_parameter("max_msgs_per_second").as_int();
-
-  if (enable_groot_monitoring) {
-    if (publisher_port <= 0 || server_port <= 0) {
-      RCLCPP_WARN(
-        get_logger(),
-        "[%s] Groot monitoring ports not provided, disabling Groot monitoring."
-        " publisher port: %d, server port: %d",
-        get_name(), publisher_port, server_port);
-    } else {
-      RCLCPP_DEBUG(
-        get_logger(),
-        "[%s] Groot monitoring: Publisher port: %d, Server port: %d, Max msgs per second: %d",
-        get_name(), publisher_port, server_port, max_msgs_per_second);
-      try {
-        publisher_zmq_.reset(
-          new BT::PublisherZMQ(
-            tree_, max_msgs_per_second, publisher_port,
-            server_port));
-      } catch (const BT::LogicError & exc) {
-        RCLCPP_ERROR(get_logger(), "ZMQ error: %s", exc.what());
-      }
-    }
-  }
-#endif
-
   finished_ = false;
   return ActionExecutorClient::on_activate(previous_state);
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 BTAction::on_deactivate(const rclcpp_lifecycle::State & previous_state)
 {
-  publisher_zmq_.reset();
   bt_minitrace_logger_.reset();
   bt_file_logger_.reset();
   tree_.haltTree();

plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp

@@ -17,14 +17,14 @@
 
 #include "CloseGripper.hpp"
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace plansys2_bt_tests
 {
 
 CloseGripper::CloseGripper(
   const std::string & xml_tag_name,
-  const BT::NodeConfiguration & conf)
+  const BT::NodeConfig & conf)
 : BT::ActionNodeBase(xml_tag_name, conf), counter_(0)
 {
 }
@@ -50,7 +50,7 @@ CloseGripper::tick()
 
 }  // namespace plansys2_bt_tests
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<plansys2_bt_tests::CloseGripper>("CloseGripper");

plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp

@@ -17,8 +17,8 @@
 
 #include <string>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace plansys2_bt_tests
 {
@@ -28,7 +28,7 @@ class CloseGripper : public BT::ActionNodeBase
 public:
   explicit CloseGripper(
     const std::string & xml_tag_name,
-    const BT::NodeConfiguration & conf);
+    const BT::NodeConfig & conf);
 
   void halt();
   BT::NodeStatus tick();

plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp

@@ -15,14 +15,14 @@
 #include <string>
 #include <memory>
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "FailureNodes.hpp"
 
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =
-    [](const std::string & name, const BT::NodeConfiguration & config)
+    [](const std::string & name, const BT::NodeConfig & config)
     {
       return std::make_unique<plansys2_bt_tests::OnTickFail>(
         name, "move", config);
@@ -32,7 +32,7 @@ BT_REGISTER_NODES(factory)
     "OnTickFail", builder);
 
   builder =
-    [](const std::string & name, const BT::NodeConfiguration & config)
+    [](const std::string & name, const BT::NodeConfig & config)
     {
       return std::make_unique<plansys2_bt_tests::OnFeedbackFail>(
         name, "move", config);