Groot2 interface (#528)

* refactored groot2 interface

* protocol updated

Author: facontidavide

include/behaviortree_cpp/loggers/groot2_protocol.h

@@ -33,21 +33,21 @@ enum RequestType : uint8_t
   // 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',
+  // Groot requests the insertion of a hook
+  HOOK_INSERT = 'I',
+  // Groot requests to remove a hook
+  HOOK_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',
+  // receive the existing hooks in JSON format
+  HOOKS_DUMP = 'D',
 
-  // Remove all breakpoints. To be done before disconnecting Groot
-  REMOVE_ALL_BREAKPOINTS = 'A',
+  // Remove all hooks. To be done before disconnecting Groot
+  REMOVE_ALL_HOOKS = 'A',
 
-  DISABLE_ALL_BREAKPOINTS = 'X',
+  DISABLE_ALL_HOOKS = 'X',
 
   UNDEFINED = 0,
 };
@@ -60,13 +60,13 @@ inline const char* ToString(const RequestType& type)
   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::HOOK_INSERT: return "hook_insert";
+  case RequestType::HOOK_REMOVE: return "hook_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::REMOVE_ALL_HOOKS: return "hooks_remove_all";
+  case RequestType::HOOKS_DUMP: return "hooks_dump";
+  case RequestType::DISABLE_ALL_HOOKS: return "disable_hooks";
 
   case RequestType::UNDEFINED: return "undefined";
   }
@@ -182,17 +182,29 @@ inline ReplyHeader DeserializeReplyHeader(const std::string& buffer)
   return header;
 }
 
-struct Breakpoint
+struct Hook
 {
-  using Ptr = std::shared_ptr<Breakpoint>;
+  using Ptr = std::shared_ptr<Hook>;
 
   // used to enable/disable the breakpoint
   bool enabled = true;
 
+  enum class Position {
+    PRE = 0,
+    POST = 1
+  };
+
+  Position position = Position::PRE;
+
   uint16_t node_uid = 0;
 
-  // interactive breakpoints are unblucked using unlockBreakpoint()
-  bool is_interactive = true;
+  enum class Mode {
+    BREAKPOINT = 0,
+    REPLACE = 1
+  };
+
+  // interactive breakpoints are unblocked using unlockBreakpoint()
+  Mode mode = Mode::BREAKPOINT;
 
   // used by interactive breakpoints to wait for unlocking
   std::condition_variable wakeup;
@@ -210,21 +222,24 @@ struct Breakpoint
 };
 
 
-void to_json(nlohmann::json& js, const Breakpoint& bp) {
+void to_json(nlohmann::json& js, const Hook& bp) {
   js = nlohmann::json {
       {"enabled", bp.enabled},
       {"uid", bp.node_uid},
-      {"interactive", bp.is_interactive},
+      {"mode", int(bp.mode)},
       {"once", bp.remove_when_done},
-      {"desired_status", toStr(bp.desired_status)}
+      {"desired_status", toStr(bp.desired_status)},
+      {"position", int(bp.position)}
   };
 }
 
-void from_json(const nlohmann::json& js, Breakpoint& bp) {
+void from_json(const nlohmann::json& js, Hook& 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);
+  bp. mode = static_cast<Hook::Mode>(js.at("mode").get<int>());
+  bp.position = static_cast<Hook::Position>(js.at("position").get<int>());
+
   const std::string desired_value = js.at("desired_status").get<std::string>();
   bp.desired_status = convertFromString<NodeStatus>(desired_value);
 }

include/behaviortree_cpp/loggers/groot2_publisher.h

@@ -13,6 +13,8 @@ class Groot2Publisher : public StatusChangeLogger
   static std::mutex used_ports_mutex;
   static std::set<unsigned> used_ports;
 
+  using Position = Monitor::Hook::Position;
+
   public:
   Groot2Publisher(const BT::Tree& tree, unsigned server_port = 1667);
 
@@ -35,15 +37,15 @@ class Groot2Publisher : public StatusChangeLogger
 
   std::vector<uint8_t> generateBlackboardsDump(const std::string& bb_list);
 
-  bool insertBreakpoint(Monitor::Breakpoint::Ptr breakpoint);
+  bool insertHook(Monitor::Hook::Ptr breakpoint);
 
-  bool unlockBreakpoint(uint16_t node_uid, NodeStatus result, bool remove);
+  bool unlockBreakpoint(Position pos, uint16_t node_uid, NodeStatus result, bool remove);
 
-  bool removeBreakpoint(uint16_t node_uid);
+  bool removeHook(Position pos, uint16_t node_uid);
 
-  void removeAllBreakpoints();
+  void removeAllHooks();
 
-  Monitor::Breakpoint::Ptr getBreakpoint(uint16_t node_uid);
+  Monitor::Hook::Ptr getHook(Position pos, uint16_t node_uid);
 
   unsigned server_port_ = 0;
   std::string server_address_;
@@ -63,17 +65,17 @@ class Groot2Publisher : public StatusChangeLogger
   std::unordered_map<std::string, std::weak_ptr<BT::Tree::Subtree>> subtrees_;
   std::unordered_map<uint16_t, std::weak_ptr<BT::TreeNode>> nodes_by_uid_;
 
-  std::mutex breakpoints_map_mutex_;
-  std::unordered_map<uint16_t, Monitor::Breakpoint::Ptr> pre_breakpoints_;
-
+  std::mutex hooks_map_mutex_;
+  std::unordered_map<uint16_t, Monitor::Hook::Ptr> pre_hooks_;
+  std::unordered_map<uint16_t, Monitor::Hook::Ptr> post_hooks_;
 
   std::chrono::system_clock::time_point last_heartbeat_;
 
   std::thread heartbeat_thread_;
 
   struct Pimpl;
   Pimpl* zmq_;
-  void enableAllBreakpoints(bool enable);
+  void enableAllHooks(bool enable);
 };
 }   // namespace BT