Add previously unfinished job manager

nexus_capabilities/include/nexus_capabilities/context.hpp

@@ -20,44 +20,53 @@
 
 #include "task.hpp"
 
-#include <nexus_common/logging.hpp>
-#include <nexus_endpoints.hpp>
-#include <nexus_orchestrator_msgs/msg/task_state.hpp>
-
-#include <behaviortree_cpp_v3/bt_factory.h>
-
-#include <rclcpp/logger.hpp>
-#include <rclcpp_action/rclcpp_action.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 
-#include <yaml-cpp/yaml.h>
-
-#include <functional>
-#include <istream>
-#include <unordered_map>
+#include <memory>
+#include <string>
+#include <unordered_set>
+#include <vector>
 
 //==============================================================================
 namespace nexus {
 
 class Context
 {
-public: using GoalHandlePtr =
-    std::shared_ptr<rclcpp_action::ServerGoalHandle<endpoints::WorkcellRequestAction::ActionType>>;
-public: using TaskState = nexus_orchestrator_msgs::msg::TaskState;
-
-  // using reference to prevent circular references
-public: rclcpp_lifecycle::LifecycleNode& node;
-public: BT::Tree bt;
-public: GoalHandlePtr goal_handle;
+  /**
+   * There are several choices we can use here, each having their own pros and cons.
+   *
+   * shared_ptr:
+   *   + Api friendly
+   *   - Will cause circular references
+   *   - Cannot create an instance in the node constructor (cannot call `shared_from_this`
+   *     in constructor)
+   *
+   * weak_ptr:
+   *   - Will almost always cause circular reference. Most ROS apis don't support weak_ptr,
+   *     attempting to `lock()` and passing the `shared_ptr` will almost always cause a
+   *     circular reference.
+   *   - Cannot create create an instance in the node constructor (cannot call `weak_from_this`
+   *     in constructor)
+   *
+   * raw_ptr:
+   *   + Api friendly
+   *   - Unsafe
+   *
+   * reference:
+   *   - Unsafe. Reference does not protect from UAF, also most ROS apis don't support references,
+   *     those that use templates may work with either shared or raw pointers.
+   *     We can't get a shared_ptr from a reference so we may end up using raw pointers.
+   *
+   * `shared_ptr` is chosen due to api friendliness, the consequences of circular reference
+   * is negligible since the node should never go out of scope.
+   */
+public: std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node;
 public: Task task;
 public: std::vector<std::string> errors;
-public: std::unique_ptr<common::BtLogging> bt_logging;
 public: std::unordered_set<std::string> signals;
-public: TaskState task_state;
-public: uint64_t tick_count = 0;
 
-public: Context(rclcpp_lifecycle::LifecycleNode& node)
-  : node(node) {}
+public: Context(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
+  : node(std::move(node)) {}
 };
 
 } // namespace nexus

nexus_capabilities/src/capabilities/detection.cpp

@@ -219,13 +219,13 @@ void DetectAllItems::_send_next_request()
 
 BT::NodeStatus GetDetection::tick()
 {
-  auto ctx = this->_ctx_mgr->current_context();
-  auto detections = this->getInput<vision_msgs::msg::Detection3DArray>(
+  const auto ctx = this->_ctx_mgr->current_context();
+  const auto detections = this->getInput<vision_msgs::msg::Detection3DArray>(
     "detections");
   if (!detections)
   {
     RCLCPP_ERROR(
-      ctx->node.get_logger(), "%s: port [detections] is required",
+      ctx->node->get_logger(), "%s: port [detections] is required",
       this->name().c_str());
     return BT::NodeStatus::FAILURE;
   }
@@ -235,7 +235,7 @@ BT::NodeStatus GetDetection::tick()
   if (!idx && !id)
   {
     RCLCPP_ERROR(
-      ctx->node.get_logger(), "%s: port [idx] or [id] is required",
+      ctx->node->get_logger(), "%s: port [idx] or [id] is required",
       this->name().c_str());
     return BT::NodeStatus::FAILURE;
   }
@@ -251,7 +251,7 @@ BT::NodeStatus GetDetection::tick()
     if (it == detections->detections.cend())
     {
       RCLCPP_ERROR(
-        ctx->node.get_logger(), "%s: cannot find detection with id [%s]",
+        ctx->node->get_logger(), "%s: cannot find detection with id [%s]",
         this->name().c_str(), id->c_str());
       return BT::NodeStatus::FAILURE;
     }
@@ -266,12 +266,13 @@ BT::NodeStatus GetDetection::tick()
 
 BT::NodeStatus GetDetectionPose::tick()
 {
-  auto ctx = this->_ctx_mgr->current_context();
-  auto detection = this->getInput<vision_msgs::msg::Detection3D>("detection");
+  const auto ctx = this->_ctx_mgr->current_context();
+  const auto detection = this->getInput<vision_msgs::msg::Detection3D>(
+    "detection");
   if (!detection)
   {
     RCLCPP_ERROR(
-      ctx->node.get_logger(), "%s: port [detection] is required",
+      ctx->node->get_logger(), "%s: port [detection] is required",
       this->name().c_str());
     return BT::NodeStatus::FAILURE;
   }

nexus_workcell_orchestrator/CMakeLists.txt

@@ -29,6 +29,7 @@ endif()
 add_library(${PROJECT_NAME}_plugin SHARED
   src/get_result.cpp
   src/get_joint_constraints.cpp
+  src/job_manager.cpp
   src/make_transform.cpp
   src/serialization.cpp
   src/set_result.cpp
@@ -91,22 +92,21 @@ if(BUILD_TESTING)
   # Adds a ament catch2 test with some common libraries.
   # Usage: nexus_add_test(<target> <source>...)
   function(nexus_add_test target)
-    ament_add_catch2(${target} ${ARGN})
+    ament_add_catch2(${target} src/main_test.cpp ${ARGN})
     target_link_libraries(${target} PRIVATE
       ${PROJECT_NAME}_plugin
       nexus_common::nexus_common_test
     )
   endfunction()
 
   nexus_add_test(serialization_tests
-    src/main_test.cpp
     src/serialization_test.cpp
   )
   nexus_add_test(task_results_tests
-    src/main_test.cpp
     src/task_results_test.cpp
   )
   nexus_add_test(transform_pose_test src/transform_pose_test.cpp)
+  nexus_add_test(job_manager_test src/job_manager_test.cpp)
 endif()
 
 ament_package()

nexus_workcell_orchestrator/src/get_result.cpp

@@ -29,7 +29,7 @@ BT::NodeStatus GetResult::tick()
   if (!key)
   {
     RCLCPP_ERROR(
-      this->_node.get_logger(), "%s: port [key] is required",
+      ctx->node->get_logger(), "%s: port [key] is required",
       this->name().c_str());
     return BT::NodeStatus::FAILURE;
   }
@@ -41,7 +41,7 @@ BT::NodeStatus GetResult::tick()
   catch (const YAML::Exception& e)
   {
     RCLCPP_ERROR(
-      this->_node.get_logger(), "[%s]: Failed to read result [%s]",
+      ctx->node->get_logger(), "[%s]: Failed to read result [%s]",
       this->name().c_str(), e.what());
     return BT::NodeStatus::FAILURE;
   }

nexus_workcell_orchestrator/src/get_result.hpp

@@ -42,14 +42,12 @@ public: static BT::PortsList providedPorts()
   }
 
 public: GetResult(const std::string& name, const BT::NodeConfiguration& config,
-    std::shared_ptr<const ContextManager> ctx_mgr,
-    rclcpp_lifecycle::LifecycleNode& node)
-  : BT::SyncActionNode{name, config}, _ctx_mgr{ctx_mgr}, _node{node} {}
+    std::shared_ptr<const ContextManager> ctx_mgr)
+  : BT::SyncActionNode{name, config}, _ctx_mgr{ctx_mgr} {}
 
 public: BT::NodeStatus tick() override;
 
 private: std::shared_ptr<const ContextManager> _ctx_mgr;
-private: rclcpp_lifecycle::LifecycleNode& _node;
 };
 
 }

nexus_workcell_orchestrator/src/job.hpp

@@ -18,33 +18,33 @@
 #ifndef NEXUS_WORKCELL_ORCHESTRATOR__JOB_HPP
 #define NEXUS_WORKCELL_ORCHESTRATOR__JOB_HPP
 
-#include <nexus_workcell_orchestrator/capability.hpp>
-#include <nexus_workcell_orchestrator/context.hpp>
+#include <nexus_capabilities/context.hpp>
+#include <nexus_common/logging.hpp>
+#include <nexus_endpoints.hpp>
+#include <nexus_orchestrator_msgs/msg/task_state.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
 
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-#include <tf2_ros/buffer.h>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <filesystem>
 
 namespace nexus::workcell_orchestrator {
 
-class Job
+struct Job
 {
-public: Job(rclcpp_lifecycle::LifecycleNode::WeakPtr node,
-    tf2_ros::Buffer::SharedPtr tf2_buffer);
-
-public: void pause() { this->_paused = true; }
-public: void unpause() { this->_paused = false; }
-public: void load_capability(const Capability& cap);
-
-private: rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
-private: tf2_ros::Buffer::SharedPtr _tf2_buffer;
-private: BT::BehaviorTreeFactory _bt_factory;
-private: std::shared_ptr<Context> _ctx;
-private: bool _paused;
+  using TaskState = nexus_orchestrator_msgs::msg::TaskState;
+  using GoalHandlePtr =
+    std::shared_ptr<rclcpp_action::ServerGoalHandle<endpoints::WorkcellRequestAction::ActionType>>;
+
+  std::shared_ptr<Context> ctx;
+  std::optional<BT::Tree> bt;
+  GoalHandlePtr goal_handle;
+  std::unique_ptr<common::BtLogging> bt_logging;
+  TaskState task_state = TaskState();
+  uint64_t tick_count = 0;
 };
 
 }
 
 #endif
-