BBInit in tree execution server for ExecuteTree action

Author: devis12

behaviortree_ros2/src/tree_execution_server.cpp

@@ -204,6 +204,12 @@ void TreeExecutionServer::execute(
       // Handle non-JSON payload case
     }
 
+    for(const auto& bb_entry : goal->bb_init)
+    {
+      RCLCPP_INFO(kLogger, "Setting blackboard key \"%s\" to \"%s\"", bb_entry.key.c_str(), bb_entry.value.c_str());
+      root_blackboard->set(bb_entry.key, bb_entry.value);
+    }
+
     // call user defined function after the tree has been created
     onTreeCreated(p_->tree);
     p_->groot_publisher.reset();

btcpp_ros2_interfaces/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(btcpp_ros2_interfaces
     "msg/NodeStatus.msg"
+    "msg/BBEntry.msg"
     "msg/CustomMsg.msg"
     "action/ExecuteTree.action"
     "action/Sleep.action")

btcpp_ros2_interfaces/action/ExecuteTree.action

@@ -2,6 +2,7 @@
 
 # Name of the tree to execute
 string target_tree
+BBEntry[] bb_init
 # Optional, implementation-dependent, payload.
 string payload
 ---

btcpp_ros2_interfaces/msg/BBEntry.msg

@@ -0,0 +1,2 @@
+string key
+string value