remove exception from capabilities

Signed-off-by: Teo Koon Peng <[email protected]>
osrf · Nov 27, 2024 · 13e02aa · 13e02aa
1 parent c0206f6
commit 13e02aa
Show file tree

Hide file tree

Showing 19 changed files with 121 additions and 105 deletions.
diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt
@@ -56,6 +56,7 @@ include(GNUInstallDirs)
 # nexus_capabilities library
 
 add_library(${PROJECT_NAME} SHARED
+  src/context.cpp
   src/conversions/pose_stamped.cpp
 )
 

diff --git a/nexus_capabilities/include/nexus_capabilities/capability.hpp b/nexus_capabilities/include/nexus_capabilities/capability.hpp
@@ -22,6 +22,7 @@
 #include "task.hpp"
 
 #include <nexus_common/bt_store.hpp>
+#include <nexus_common/error.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
@@ -54,7 +55,7 @@ class Capability
    * @param bt_factory Must be valid for the lifetime of the capability.
    * @throws StateTransitionError if transition fails.
    */
-  virtual void configure(
+  [[nodiscard]] virtual common::Result<void> configure(
     rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) = 0;

diff --git a/nexus_capabilities/include/nexus_capabilities/context.hpp b/nexus_capabilities/include/nexus_capabilities/context.hpp
@@ -20,6 +20,11 @@
 
 #include "task.hpp"
 
+#include <nexus_common/error.hpp>
+#include <nexus_endpoints.hpp>
+#include <nexus_orchestrator_msgs/msg/task_progress.hpp>
+
+#include <rclcpp_action/rclcpp_action.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 
 #include <memory>
@@ -32,6 +37,9 @@ namespace nexus {
 
 class Context
 {
+public: using GoalHandlePtr =
+    std::shared_ptr<rclcpp_action::ServerGoalHandle<endpoints::WorkcellRequestAction::ActionType>>;
+
   /**
    * There are several choices we can use here, each having their own pros and cons.
    *
@@ -65,8 +73,15 @@ public: Task task;
 public: std::vector<std::string> errors;
 public: std::unordered_set<std::string> signals;
 
-public: Context(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
-  : node(std::move(node)) {}
+public: Context(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node,
+    GoalHandlePtr goal_handle)
+  : node(std::move(node)), _goal_handle(std::move(goal_handle)) {}
+
+public: common::Result<void> publish_feedback(
+    const nexus_orchestrator_msgs::msg::TaskProgress& progress,
+    const std::string& msg = "");
+
+private: GoalHandlePtr _goal_handle;
 };
 
 } // namespace nexus

diff --git a/nexus_capabilities/include/nexus_capabilities/exceptions.hpp b/nexus_capabilities/include/nexus_capabilities/exceptions.hpp
diff --git a/nexus_capabilities/src/capabilities/detection_capability.cpp b/nexus_capabilities/src/capabilities/detection_capability.cpp
@@ -37,7 +37,7 @@ void DetectionCapability::declare_params(
 }
 
 //==============================================================================
-void DetectionCapability::configure(
+common::Result<void> DetectionCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> ctx_mgr,
   BT::BehaviorTreeFactory& bt_factory)
@@ -57,35 +57,21 @@ void DetectionCapability::configure(
 
   bt_factory.registerBuilder<DetectOffset>(
     "detection.DetectOffset",
-    [this, ctx_mgr, w_node = std::weak_ptr{node}](const std::string& name,
+    [this, ctx_mgr, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
-      auto node = w_node.lock();
-      if (!node)
-      {
-        std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl;
-        std::terminate();
-      }
-
       return std::make_unique<DetectOffset>(name, config,
-      node->get_logger(), ctx_mgr, [this, node](const std::string& detector)
+      node->get_logger(), ctx_mgr, [this](const std::string& detector)
       {
         return this->_clients.at(detector);
       });
     });
 
   bt_factory.registerBuilder<DetectAllItems>(
     "detection.DetectAllItems",
-    [this, ctx_mgr, w_node = std::weak_ptr{node}](const std::string& name,
+    [this, ctx_mgr, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
-      auto node = w_node.lock();
-      if (!node)
-      {
-        std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl;
-        std::terminate();
-      }
-
       return std::make_unique<DetectAllItems>(name, config, node, ctx_mgr,
       [this](const std::string& detector)
       {
@@ -106,6 +92,8 @@ void DetectionCapability::configure(
     {
       return std::make_unique<GetDetectionPose>(name, config, ctx_mgr);
     });
+
+  return common::Result<void>();
 }
 
 //==============================================================================

diff --git a/nexus_capabilities/src/capabilities/detection_capability.hpp b/nexus_capabilities/src/capabilities/detection_capability.hpp
@@ -20,6 +20,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -45,7 +46,7 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final;
   /**
    * @copydoc Capability::configure
    */
-public: void configure(
+public: common::Result<void> configure(
     rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;

diff --git a/nexus_capabilities/src/capabilities/dispense_item_capability.cpp b/nexus_capabilities/src/capabilities/dispense_item_capability.cpp
@@ -19,8 +19,6 @@
 
 #include "dispense_item_task_data.hpp"
 
-#include <nexus_capabilities/exceptions.hpp>
-
 #include <yaml-cpp/exceptions.h>
 
 namespace nexus::capabilities {
@@ -47,7 +45,7 @@ void DispenseItemCapability::declare_params(
 }
 
 //==============================================================================
-void DispenseItemCapability::configure(
+common::Result<void> DispenseItemCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
@@ -76,6 +74,8 @@ void DispenseItemCapability::configure(
       return std::make_unique<DispenseItem>(name, config,
       node, this->_dispensers, std::chrono::milliseconds{dispenser_timeout});
     });
+
+  return common::Result<void>();
 }
 
 }

diff --git a/nexus_capabilities/src/capabilities/dispense_item_capability.hpp b/nexus_capabilities/src/capabilities/dispense_item_capability.hpp
@@ -22,6 +22,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -48,7 +49,8 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final;
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 

diff --git a/nexus_capabilities/src/capabilities/execute_trajectory.hpp b/nexus_capabilities/src/capabilities/execute_trajectory.hpp
@@ -35,7 +35,7 @@ namespace nexus::capabilities {
  *   trajectory |moveit_msgs::msg::RobotTrajectory| The trajectory to execute.
  */
 class ExecuteTrajectory : public common
-  ::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode*,
+  ::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode::SharedPtr,
     endpoints::ControllerRobotTrajectoryAction::ActionType>
 {
 public: using ActionType =
@@ -54,9 +54,10 @@ public: static BT::PortsList providedPorts();
    */
 public: inline ExecuteTrajectory(const std::string& name,
     const BT::NodeConfiguration& config,
-    rclcpp_lifecycle::LifecycleNode& node)
-  : common::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode*, ActionType>(
-      name, config, &node) {}
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+  : common::ActionClientBtNode<rclcpp_lifecycle::LifecycleNode::SharedPtr,
+      ActionType>(
+      name, config, std::move(node)) {}
 
 protected: std::string get_action_name() const
   override

diff --git a/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp b/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp
@@ -25,25 +25,19 @@
 
 namespace nexus::capabilities {
 
-void ExecuteTrajectoryCapability::configure(
+common::Result<void> ExecuteTrajectoryCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
 {
   bt_factory.registerBuilder<ExecuteTrajectory>(
     "execute_trajectory.ExecuteTrajectory",
-    [w_node = std::weak_ptr{node}](const std::string& name,
+    [node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
-      auto node = w_node.lock();
-      if (!node)
-      {
-        std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl;
-        std::terminate();
-      }
-
-      return std::make_unique<ExecuteTrajectory>(name, config, *node);
+      return std::make_unique<ExecuteTrajectory>(name, config, node);
     });
+  return common::Result<void>();
 }
 
 }

diff --git a/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp b/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp
@@ -22,6 +22,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -48,7 +49,8 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& /* node */) final
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 

diff --git a/nexus_capabilities/src/capabilities/gripper_capability.cpp b/nexus_capabilities/src/capabilities/gripper_capability.cpp
@@ -31,7 +31,7 @@ void GripperCapability::declare_params(rclcpp_lifecycle::LifecycleNode& node)
   node.declare_parameter("grippers", std::vector<std::string>{}, desc);
 }
 
-void GripperCapability::configure(
+common::Result<void> GripperCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
@@ -48,6 +48,8 @@ void GripperCapability::configure(
     {
       return std::make_unique<GripperControl>(name, config, *node);
     });
+
+  return common::Result<void>();
 }
 
 }

diff --git a/nexus_capabilities/src/capabilities/gripper_capability.hpp b/nexus_capabilities/src/capabilities/gripper_capability.hpp
@@ -20,6 +20,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -45,7 +46,7 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final;
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;
 

diff --git a/nexus_capabilities/src/capabilities/plan_motion_capability.cpp b/nexus_capabilities/src/capabilities/plan_motion_capability.cpp
@@ -19,15 +19,13 @@
 
 #include "plan_motion.hpp"
 
-#include <nexus_capabilities/exceptions.hpp>
-
 #include <tf2_ros/transform_broadcaster.h>
 
 #include <memory>
 
 namespace nexus::capabilities {
 
-void PlanMotionCapability::configure(
+common::Result<void> PlanMotionCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> /* ctx_mgr */,
   BT::BehaviorTreeFactory& bt_factory)
@@ -43,6 +41,7 @@ void PlanMotionCapability::configure(
       return std::make_unique<PlanMotion>(name, config, *node, this->_client,
       tf_broadcaster);
     });
+  return common::Result<void>();
 }
 
 }

diff --git a/nexus_capabilities/src/capabilities/plan_motion_capability.hpp b/nexus_capabilities/src/capabilities/plan_motion_capability.hpp
@@ -20,6 +20,7 @@
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
+#include <nexus_common/error.hpp>
 #include <nexus_endpoints.hpp>
 
 #include <behaviortree_cpp_v3/bt_factory.h>
@@ -45,7 +46,8 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& /* node */) final
   /**
    * @copydoc Capability::configure
    */
-public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+public: common::Result<void> configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::shared_ptr<const ContextManager> ctx_mgr,
     BT::BehaviorTreeFactory& bt_factory) final;