Restore main.xml

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

nexus_integration_tests/config/system_bts/main.xml

@@ -0,0 +1,21 @@
+<root main_tree_to_execute="main">
+  <BehaviorTree ID="main">
+    <ReactiveSequence>
+      <IsPauseTriggered paused="{paused}"/>
+      <Fallback>
+        <PausableSequence pause="{paused}">
+          <ForEachTask task="{task}" workcell="{workcell}">
+            <ExecuteTask task="{task}" workcell="{workcell}"/>
+          </ForEachTask>
+          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
+        </PausableSequence>
+        <PausableSequence pause="{paused}">
+          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
+          <AlwaysFailure/>
+        </PausableSequence>
+      </Fallback>
+    </ReactiveSequence>
+  </BehaviorTree>
+</root>

nexus_integration_tests/config/system_bts/pick_and_place.xml

@@ -2,30 +2,14 @@
   <BehaviorTree ID="pick_and_place">
     <ReactiveSequence>
       <IsPauseTriggered paused="{paused}"/>
-      <Fallback>
-        <PausableSequence pause="{paused}">
-          <ForEachTask task="{task}" workcell="{workcell}">
-            <ReactiveSequence>
-              <IsPauseTriggered paused="{paused}"/>
-              <Parallel success_threshold="-1">
-                <Sequence>
-                  <BidTransporter name="bid_transporterasdasd" destination="{workcell}" result="{transporter}"/>
-                  <TransporterRequest name="transporter_request" transporter="{transporter}" destination="{workcell}"/>
-                  <SendSignal task="{task}" signal="transporter_done"/>
-                </Sequence>
-                <WorkcellRequest name="workcell_request" workcell="{workcell}" task="{task}"/>
-              </Parallel>
-            </ReactiveSequence>
-          </ForEachTask>
-          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
-          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
-        </PausableSequence>
-        <PausableSequence pause="{paused}">
-          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
-          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
-          <AlwaysFailure/>
-        </PausableSequence>
-      </Fallback>
+      <Parallel success_threshold="-1">
+        <Sequence>
+          <BidTransporter name="bid_transporterasdasd" destination="{workcell}" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="{workcell}"/>
+          <SendSignal task="{task}" signal="transporter_done"/>
+        </Sequence>
+        <WorkcellRequest name="workcell_request" workcell="{workcell}" task="{task}"/>
+      </Parallel>
     </ReactiveSequence>
   </BehaviorTree>
 </root>

nexus_integration_tests/launch/control_center.launch.py

@@ -128,7 +128,11 @@ def launch_setup(context, *args, **kwargs):
                     FindPackageShare("nexus_integration_tests"),
                     "/config/system_bts",
                 ),
-                "bt_filename": "pick_and_place.xml",
+                "remap_task_types":
+                    """{
+                        pick_and_place: [place_on_conveyor, pick_from_conveyor],
+                    }""",
+                "bt_filename": "main.xml",
                 "max_jobs": 2,
             }
         ],

nexus_system_orchestrator/CMakeLists.txt

@@ -19,6 +19,7 @@ set(CMAKE_CXX_FLAGS "-Wall -Wpedantic")
 
 add_library(${PROJECT_NAME}_plugin SHARED
   src/bid_transporter.cpp
+  src/execute_task.cpp
   src/for_each_task.cpp
   src/send_signal.cpp
   src/system_orchestrator.cpp

nexus_system_orchestrator/src/context.hpp

@@ -43,6 +43,8 @@ public: rclcpp_lifecycle::LifecycleNode& node;
 public: std::string job_id;
 public: WorkOrder wo;
 public: std::vector<WorkcellTask> tasks;
+public: std::shared_ptr<const std::unordered_map<std::string,
+    std::string>> task_remaps;
   /**
    * Map of task ids and their assigned workcell ids.
    */

nexus_system_orchestrator/src/execute_task.cpp

@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2023 Johnson & Johnson
+ *
+ * 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.
+ *
+ */
+
+#include "execute_task.hpp"
+
+namespace nexus::system_orchestrator {
+
+BT::NodeStatus ExecuteTask::onStart()
+{
+  auto task = this->getInput<Task>("task");
+  if (!task)
+  {
+    RCLCPP_ERROR(
+      this->_ctx->node.get_logger(), "%s: [task] port is required",
+      this->name().c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  auto workcell = this->getInput<std::string>("workcell");
+  if (!workcell)
+  {
+    RCLCPP_ERROR(
+      this->_ctx->node.get_logger(), "%s: [workcell] port is required",
+      this->name().c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Remap the BT filename to load if one is provided.
+  std::string bt_name = task->type;
+  auto it = _ctx->task_remaps->find(task->type);
+  if (it != _ctx->task_remaps->end())
+  {
+    RCLCPP_DEBUG(
+      _ctx->node.get_logger(),
+      "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]",
+      it->second.c_str(),
+      task->type.c_str()
+    );
+    bt_name = it->second;
+  }
+  std::filesystem::path task_bt_path(this->_bt_path / (bt_name + ".xml"));
+  if (!std::filesystem::is_regular_file(task_bt_path))
+  {
+    RCLCPP_ERROR(
+      this->_ctx->node.get_logger(), "%s: no behavior tree to execute task type [%s]",
+      this->name().c_str(), task->type.c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  this->_bt = std::make_unique<BT::Tree>(this->_bt_factory->createTreeFromFile(
+        task_bt_path));
+  this->_bt->rootBlackboard()->set("task", *task);
+  this->_bt->rootBlackboard()->set("workcell", *workcell);
+
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus ExecuteTask::onRunning()
+{
+  return this->_bt->tickRoot();
+}
+
+void ExecuteTask::onHalted()
+{
+  this->_bt->haltTree();
+}
+
+}

nexus_system_orchestrator/src/execute_task.hpp

@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2023 Johnson & Johnson
+ *
+ * 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.
+ *
+ */
+
+#ifndef NEXUS_SYSTEM_ORCHESTRATOR__EXECUTE_TASK_HPP
+#define NEXUS_SYSTEM_ORCHESTRATOR__EXECUTE_TASK_HPP
+
+#include "context.hpp"
+
+#include <nexus_orchestrator_msgs/msg/workcell_task.hpp>
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <behaviortree_cpp_v3/bt_factory.h>
+
+#include <filesystem>
+
+namespace nexus::system_orchestrator {
+
+/**
+ * Searches for a behavior tree to execute a task and run it as a sub tree.
+ *
+ * The behavior tree used is based on the task type after remapping and the
+ * filename of the behavior tree without extension.
+ *
+ * Input Ports:
+ *   task |nexus_orchestrator_msgs::msg::WorkcellTask| The task to execute.
+ *   workcell |std::string| Workcell to execute on.
+ */
+class ExecuteTask : public BT::StatefulActionNode
+{
+public: using Task = nexus_orchestrator_msgs::msg::WorkcellTask;
+
+public: static BT::PortsList providedPorts()
+  {
+    return { BT::InputPort<Task>("task"),
+      BT::InputPort<std::string>("workcell") };
+  }
+
+public: ExecuteTask(const std::string& name,
+    const BT::NodeConfiguration& config,
+    std::shared_ptr<Context> ctx,
+    std::filesystem::path bt_path,
+    std::shared_ptr<BT::BehaviorTreeFactory> bt_factory)
+  : BT::StatefulActionNode(name, config), _ctx(std::move(ctx)), _bt_path(std::move(
+        bt_path)),
+    _bt_factory(std::move(bt_factory)) {}
+
+protected: BT::NodeStatus onStart() override;
+
+protected: BT::NodeStatus onRunning() override;
+
+protected: void onHalted() override;
+
+private: std::shared_ptr<Context> _ctx;
+private: std::filesystem::path _bt_path;
+private: std::shared_ptr<BT::BehaviorTreeFactory> _bt_factory;
+private: std::unique_ptr<BT::Tree> _bt;
+};
+
+}
+
+#endif

nexus_system_orchestrator/src/system_orchestrator.cpp

@@ -20,6 +20,7 @@
 #include "bid_transporter.hpp"
 #include "context.hpp"
 #include "exceptions.hpp"
+#include "execute_task.hpp"
 #include "for_each_task.hpp"
 #include "job.hpp"
 #include "send_signal.hpp"
@@ -70,7 +71,7 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
     ParameterDescriptor desc;
     desc.read_only = true;
     desc.description =
-      "Path to a directory containing behavior trees. Each file in the directory should be a behavior tree xml.";
+      "Path to a directory containing behavior trees. Each file in the directory should be a behavior tree xml, the file name denotes the task type for that behavior tree.";
     this->_bt_path = this->declare_parameter("bt_path", "", desc);
     if (this->_bt_path.empty())
     {
@@ -90,10 +91,6 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
     desc.description =
       "Filename of the main behavior tree to run. Paths will be resolved relative to the \"bt_path\" parameter. Defaults to \"main.xml\".";
     this->_bt_filename = this->declare_parameter("bt_filename", "main.xml", desc);
-    if (this->_bt_path.empty())
-    {
-      throw std::runtime_error("param [bt_path] is required");
-    }
 
     if (!this->_bt_filename_valid(this->_bt_filename))
     {
@@ -102,6 +99,26 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
     }
   }
 
+  {
+    _task_remaps =
+      std::make_shared<std::unordered_map<std::string, std::string>>();
+    ParameterDescriptor desc;
+    desc.read_only = true;
+    desc.description =
+      "A yaml containing a dictionary of task types and an array of remaps.";
+    const auto yaml = this->declare_parameter("remap_task_types", "", desc);
+    const auto remaps = YAML::Load(yaml);
+    for (const auto& n : remaps)
+    {
+      const auto task_type = n.first.as<std::string>();
+      const auto& mappings = n.second;
+      for (const auto& m : mappings)
+      {
+        this->_task_remaps->emplace(m.as<std::string>(), task_type);
+      }
+    }
+  }
+
   {
     ParameterDescriptor desc;
     desc.read_only = true;
@@ -150,6 +167,7 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
         }
         return result;
       });
+
 }
 
 auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous)
@@ -502,6 +520,14 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo,
       this->get_logger(), ctx);
     });
 
+  bt_factory->registerBuilder<ExecuteTask>("ExecuteTask",
+    [this, ctx, bt_factory](const std::string& name,
+    const BT::NodeConfiguration& config)
+    {
+      return std::make_unique<ExecuteTask>(name, config, ctx, this->_bt_path,
+      bt_factory);
+    });
+
   bt_factory->registerBuilder<SendSignal>("SendSignal",
     [ctx](const std::string& name, const BT::NodeConfiguration& config)
     {
@@ -519,7 +545,7 @@ void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)
 
   // using `new` because make_shared does not work with aggregate initializer
   std::shared_ptr<Context> ctx{new Context{*this,
-      goal.order.id, wo, tasks,
+      goal.order.id, wo, tasks, this->_task_remaps,
       std::unordered_map<std::string, std::string>{},
       this->_workcell_sessions,
       this->_transporter_sessions, {}, nullptr,

nexus_system_orchestrator/src/system_orchestrator.hpp

@@ -96,6 +96,8 @@ class SystemOrchestrator : public
   std::unique_ptr<lifecycle_manager::LifecycleManager<>> _lifecycle_mgr{nullptr};
   rclcpp::TimerBase::SharedPtr _pre_configure_timer;
   rclcpp::SubscriptionBase::SharedPtr _estop_sub;
+  // mapping of mapped task type and the original
+  std::shared_ptr<std::unordered_map<std::string, std::string>> _task_remaps;
   std::shared_ptr<OnSetParametersCallbackHandle> _param_cb_handle;
 
   /**