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
-