merging groot2 publisher

Author: facontidavide

CMakeLists.txt

@@ -117,6 +117,7 @@ list(APPEND BT_SOURCE
     src/loggers/bt_file_logger.cpp
     src/loggers/bt_minitrace_logger.cpp
     src/loggers/bt_observer.cpp
+    src/loggers/groot2_publisher.cpp
 
     3rdparty/tinyxml2/tinyxml2.cpp
     3rdparty/minitrace/minitrace.cpp

examples/CMakeLists.txt

@@ -27,7 +27,7 @@ CompileExample("t08_additional_node_args")
 CompileExample("t09_scripting")
 CompileExample("t10_observer")
 CompileExample("t11_replace_rules")
-
+CompileExample("t12_groot_howto")
 
 CompileExample("ex01_wrap_legacy")
 CompileExample("ex02_runtime_ports")

examples/t12_groot_howto.cpp

@@ -0,0 +1,74 @@
+#include "crossdoor_nodes.h"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
+#include "behaviortree_cpp/xml_parsing.h"
+
+/** We are using the same example in Tutorial 5,
+ *  But this time we also show how to connect
+ */
+
+// clang-format off
+
+static const char* xml_text = R"(
+<root BTCPP_format="4">
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <Fallback>
+                <Inverter>
+                    <IsDoorClosed/>
+                </Inverter>
+                <SubTree ID="DoorClosed"/>
+            </Fallback>
+            <PassThroughDoor/>
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="DoorClosed">
+        <Fallback>
+            <OpenDoor/>
+            <RetryUntilSuccessful num_attempts="5">
+                <PickLock/>
+            </RetryUntilSuccessful>
+            <SmashDoor/>
+        </Fallback>
+    </BehaviorTree>
+
+</root>
+ )";
+
+// clang-format on
+
+int main()
+{
+  BT::BehaviorTreeFactory factory;
+
+  CrossDoor cross_door;
+  cross_door.registerNodes(factory);
+
+  // Groot2 editor requires a model of your registered Nodes.
+  // You don't need to write that by hand, if can be automatically
+  // generated using this command and imported.
+
+  std::string xml_models = BT::writeTreeNodesModelXML(factory);
+  std::cout << " ---------- XML file containing models ----------\n"
+            << xml_models
+            << "-------------------------------------------------\n";
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree");
+
+  // Connect the Groot2Publisher. This will allow Groot2 to
+  // get the tree and poll status updates.
+  BT::Groot2Publisher publisher(tree);
+
+  while(1)
+  {
+    std::cout << "Start" << std::endl;
+    cross_door.reset();
+    tree.tickWhileRunning();
+    std::this_thread::sleep_for(std::chrono::milliseconds(3000));
+  }
+
+  return 0;
+}

include/behaviortree_cpp/loggers/bt_zmq_publisher.h

@@ -7,7 +7,8 @@
 
 namespace BT
 {
-class PublisherZMQ : public StatusChangeLogger
+class [[deprecated("Please use Groot2Publisher instead")]]
+PublisherZMQ : public StatusChangeLogger
 {
   static std::atomic<bool> ref_count;
 

include/behaviortree_cpp/loggers/groot2_protocol.h

@@ -0,0 +1,232 @@
+#pragma once
+
+#include <cstdint>
+#include <array>
+#include <cstring>
+#include <stdexcept>
+#include <random>
+#include <memory>
+#include <condition_variable>
+#include <mutex>
+#include "behaviortree_cpp/basic_types.h"
+#include "behaviortree_cpp/utils/json.hpp"
+
+namespace BT::Monitor {
+
+/*
+ * All the messages exchange with the BT executor are multipart ZMQ request-replies.
+ *
+ * The first part of the request and the reply have fixed size and are described below.
+ * The request and reply must have the same value of the fields:
+ *
+ *  - request_id
+ *  - request_type
+ *  - protocol_id
+ */
+
+enum RequestType : uint8_t
+{
+  // Request the entire tree defintion as XML
+  FULLTREE = 'T',
+  // Request the staus of all the nodes
+  STATUS = 'S',
+  // retrieve the valus in a set of blackboards
+  BLACKBOARD = 'B',
+
+  // Groot requests the insertion of a breakpoint
+  BREAKPOINT_INSERT = 'I',
+  // Groot requests to remove a breakpoint
+  BREAKPOINT_REMOVE = 'R',
+  // Notify Groot that we reached a breakpoint
+  BREAKPOINT_REACHED = 'N',
+  // Groot will unlock a breakpoint
+  BREAKPOINT_UNLOCK = 'U',
+  // receive the existing breakpoints in JSON format
+  BREAKPOINTS_DUMP = 'D',
+
+  // Remove all breakpoints. To be done before disconnecting Groot
+  REMOVE_ALL_BREAKPOINTS = 'A',
+
+  DISABLE_ALL_BREAKPOINTS = 'X',
+
+  UNDEFINED = 0,
+};
+
+inline const char* ToString(const RequestType& type)
+{
+  switch(type)
+  {
+  case RequestType::FULLTREE: return "full_tree";
+  case RequestType::STATUS: return "status";
+  case RequestType::BLACKBOARD: return "blackboard";
+
+  case RequestType::BREAKPOINT_INSERT: return "breakpoint_insert";
+  case RequestType::BREAKPOINT_REMOVE: return "breakpoint_remove";
+  case RequestType::BREAKPOINT_REACHED: return "breakpoint_reached";
+  case RequestType::BREAKPOINT_UNLOCK: return "breakpoint_unlock";
+  case RequestType::REMOVE_ALL_BREAKPOINTS: return "breakpoint_remove_all";
+  case RequestType::BREAKPOINTS_DUMP: return "breakpoints_dump";
+  case RequestType::DISABLE_ALL_BREAKPOINTS: return "disable_breakpoints";
+
+  case RequestType::UNDEFINED: return "undefined";
+  }
+  return "undefined";
+}
+
+constexpr uint8_t kProtocolID = 1;
+using TreeUniqueUUID = std::array<char, 16>;
+
+struct RequestHeader
+{
+  uint32_t unique_id = 0;
+  uint8_t protocol = kProtocolID;
+  RequestType type = RequestType::UNDEFINED;
+
+  static size_t size() {
+    return sizeof(uint32_t) + sizeof(uint8_t) + sizeof(uint8_t);
+  }
+
+  RequestHeader() = default;
+
+  RequestHeader(RequestType type): type(type)
+  {
+    // a random number for request_id will do
+    static std::random_device rd;
+    std::mt19937 mt(rd());
+    std::uniform_int_distribution<uint32_t> dist;
+    unique_id = dist(mt);
+  }
+
+  bool operator==(const RequestHeader& other) const
+  {
+    return type == other.type &&
+           protocol == other.protocol &&
+           unique_id == other.unique_id;
+  }
+  bool operator!=(const RequestHeader& other) const
+  {
+    return !(*this == other);
+  }
+};
+
+struct ReplyHeader
+{
+  RequestHeader request;
+  TreeUniqueUUID tree_id;
+
+  static size_t size() {
+    return RequestHeader::size() + 16;
+  }
+
+  ReplyHeader() {
+    tree_id.fill(0);
+  }
+};
+
+template <typename T> inline
+unsigned Serialize(char* buffer, unsigned offset, T value)
+{
+  memcpy(buffer + offset, &value, sizeof(T));
+  return sizeof(T);
+}
+
+template <typename T> inline
+    unsigned Deserialize(const char* buffer, unsigned offset, T& value)
+{
+  memcpy(reinterpret_cast<char*>(&value), buffer + offset, sizeof(T));
+  return sizeof(T);
+}
+
+
+inline std::string SerializeHeader(const RequestHeader& header)
+{
+  std::string buffer;
+  buffer.resize(6);
+  unsigned offset = 0;
+  offset += Serialize(buffer.data(), offset, header.protocol);
+  offset += Serialize(buffer.data(), offset, uint8_t(header.type));
+  offset += Serialize(buffer.data(), offset, header.unique_id);
+  return buffer;
+}
+
+inline std::string SerializeHeader(const ReplyHeader& header)
+{
+  // copy the first part directly (6 bytes)
+  std::string buffer = SerializeHeader(header.request);
+  // add the following 16 bytes
+  unsigned const offset = 6;
+  buffer.resize(offset + 16);
+  Serialize(buffer.data(), offset, header.tree_id);
+  return buffer;
+}
+
+inline RequestHeader DeserializeRequestHeader(const std::string& buffer)
+{
+  RequestHeader header;
+  unsigned offset = 0;
+  offset += Deserialize(buffer.data(), offset, header.protocol);
+  uint8_t type;
+  offset += Deserialize(buffer.data(), offset, type);
+  header.type = static_cast<Monitor::RequestType>(type);
+  offset += Deserialize(buffer.data(), offset, header.unique_id);
+  return header;
+}
+
+
+inline ReplyHeader DeserializeReplyHeader(const std::string& buffer)
+{
+  ReplyHeader header;
+  header.request = DeserializeRequestHeader(buffer);
+  unsigned const offset = 6;
+  Deserialize(buffer.data(), offset, header.tree_id);
+  return header;
+}
+
+struct Breakpoint
+{
+  using Ptr = std::shared_ptr<Breakpoint>;
+
+  // used to enable/disable the breakpoint
+  bool enabled = true;
+
+  uint16_t node_uid = 0;
+
+  // interactive breakpoints are unblucked using unlockBreakpoint()
+  bool is_interactive = true;
+
+  // used by interactive breakpoints to wait for unlocking
+  std::condition_variable wakeup;
+
+  std::mutex mutex;
+
+  // set to true to unlock an interactive breakpoint
+  bool ready = false;
+
+  // once finished self-destroy
+  bool remove_when_done = false;
+
+  // result to be returned
+  NodeStatus desired_status = NodeStatus::SKIPPED;
+};
+
+
+void to_json(nlohmann::json& js, const Breakpoint& bp) {
+  js = nlohmann::json {
+      {"enabled", bp.enabled},
+      {"uid", bp.node_uid},
+      {"interactive", bp.is_interactive},
+      {"once", bp.remove_when_done},
+      {"desired_status", toStr(bp.desired_status)}
+  };
+}
+
+void from_json(const nlohmann::json& js, Breakpoint& bp) {
+  js.at("enabled").get_to(bp.enabled);
+  js.at("uid").get_to(bp.node_uid);
+  js.at("interactive").get_to(bp.is_interactive);
+  js.at("once").get_to(bp.remove_when_done);
+  const std::string desired_value = js.at("desired_status").get<std::string>();
+  bp.desired_status = convertFromString<NodeStatus>(desired_value);
+}
+
+}