diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt index 6f9d4a3..c9e590e 100644 --- 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 index 830febf..5469dc4 100644 --- a/nexus_capabilities/include/nexus_capabilities/capability.hpp +++ b/nexus_capabilities/include/nexus_capabilities/capability.hpp @@ -22,6 +22,7 @@ #include "task.hpp" #include +#include #include #include @@ -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 configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr 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 index 75d5cfa..a8c1011 100644 --- a/nexus_capabilities/include/nexus_capabilities/context.hpp +++ b/nexus_capabilities/include/nexus_capabilities/context.hpp @@ -20,6 +20,11 @@ #include "task.hpp" +#include +#include +#include + +#include #include #include @@ -32,6 +37,9 @@ namespace nexus { class Context { +public: using GoalHandlePtr = + std::shared_ptr>; + /** * 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 errors; public: std::unordered_set signals; -public: Context(std::shared_ptr node) - : node(std::move(node)) {} +public: Context(std::shared_ptr node, + GoalHandlePtr goal_handle) + : node(std::move(node)), _goal_handle(std::move(goal_handle)) {} + +public: common::Result publish_feedback( + const nexus_orchestrator_msgs::msg::TaskProgress& progress, + const std::string& msg = ""); + +private: GoalHandlePtr _goal_handle; }; } // namespace nexus diff --git a/nexus_capabilities/src/capabilities/detection_capability.cpp b/nexus_capabilities/src/capabilities/detection_capability.cpp index c546411..05975f3 100644 --- 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 DetectionCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr ctx_mgr, BT::BehaviorTreeFactory& bt_factory) @@ -57,18 +57,11 @@ void DetectionCapability::configure( bt_factory.registerBuilder( "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(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); }); @@ -76,16 +69,9 @@ void DetectionCapability::configure( bt_factory.registerBuilder( "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(name, config, node, ctx_mgr, [this](const std::string& detector) { @@ -106,6 +92,8 @@ void DetectionCapability::configure( { return std::make_unique(name, config, ctx_mgr); }); + + return common::Result(); } //============================================================================== diff --git a/nexus_capabilities/src/capabilities/detection_capability.hpp b/nexus_capabilities/src/capabilities/detection_capability.hpp index ced80a5..81c72c1 100644 --- a/nexus_capabilities/src/capabilities/detection_capability.hpp +++ b/nexus_capabilities/src/capabilities/detection_capability.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -45,7 +46,7 @@ public: void declare_params(rclcpp_lifecycle::LifecycleNode& node) final; /** * @copydoc Capability::configure */ -public: void configure( +public: common::Result configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr 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 index 1277d8c..9288d3c 100644 --- 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 - #include namespace nexus::capabilities { @@ -47,7 +45,7 @@ void DispenseItemCapability::declare_params( } //============================================================================== -void DispenseItemCapability::configure( +common::Result DispenseItemCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr /* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) @@ -76,6 +74,8 @@ void DispenseItemCapability::configure( return std::make_unique(name, config, node, this->_dispensers, std::chrono::milliseconds{dispenser_timeout}); }); + + return common::Result(); } } diff --git a/nexus_capabilities/src/capabilities/dispense_item_capability.hpp b/nexus_capabilities/src/capabilities/dispense_item_capability.hpp index 15b5923..b01faaf 100644 --- a/nexus_capabilities/src/capabilities/dispense_item_capability.hpp +++ b/nexus_capabilities/src/capabilities/dispense_item_capability.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -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 configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr 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 index 768717e..3210187 100644 --- 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 { 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( - name, config, &node) {} + rclcpp_lifecycle::LifecycleNode::SharedPtr node) + : common::ActionClientBtNode( + 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 index 8d66b25..d04e0ec 100644 --- 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 ExecuteTrajectoryCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr /* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) { bt_factory.registerBuilder( "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(name, config, *node); + return std::make_unique(name, config, node); }); + return common::Result(); } } diff --git a/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp b/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp index d544f5e..d818712 100644 --- a/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp +++ b/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -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 configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr 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 index b4ae30d..3a55abf 100644 --- 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{}, desc); } -void GripperCapability::configure( +common::Result GripperCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr /* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) @@ -48,6 +48,8 @@ void GripperCapability::configure( { return std::make_unique(name, config, *node); }); + + return common::Result(); } } diff --git a/nexus_capabilities/src/capabilities/gripper_capability.hpp b/nexus_capabilities/src/capabilities/gripper_capability.hpp index 82e541a..02fe6be 100644 --- a/nexus_capabilities/src/capabilities/gripper_capability.hpp +++ b/nexus_capabilities/src/capabilities/gripper_capability.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -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 configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr 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 index 7745047..da3e1e1 100644 --- 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 - #include #include namespace nexus::capabilities { -void PlanMotionCapability::configure( +common::Result PlanMotionCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr /* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) @@ -43,6 +41,7 @@ void PlanMotionCapability::configure( return std::make_unique(name, config, *node, this->_client, tf_broadcaster); }); + return common::Result(); } } diff --git a/nexus_capabilities/src/capabilities/plan_motion_capability.hpp b/nexus_capabilities/src/capabilities/plan_motion_capability.hpp index 2622965..f2c7a2b 100644 --- a/nexus_capabilities/src/capabilities/plan_motion_capability.hpp +++ b/nexus_capabilities/src/capabilities/plan_motion_capability.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -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 configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr ctx_mgr, BT::BehaviorTreeFactory& bt_factory) final; diff --git a/nexus_capabilities/src/context.cpp b/nexus_capabilities/src/context.cpp new file mode 100644 index 0000000..d2d5f36 --- /dev/null +++ b/nexus_capabilities/src/context.cpp @@ -0,0 +1,46 @@ +/* + * 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 "nexus_capabilities/context.hpp" + +#include + +namespace nexus { + +common::Result Context::publish_feedback( + const nexus_orchestrator_msgs::msg::TaskProgress& progress, + const std::string& msg) +{ + endpoints::WorkcellRequestAction::ActionType::Feedback::SharedPtr feedback_msg; + feedback_msg->state.task_id = this->task.id; + feedback_msg->state.workcell_id = this->node->get_name(); + feedback_msg->state.status = + nexus_orchestrator_msgs::msg::TaskState::STATUS_RUNNING; + feedback_msg->state.progress = progress; + feedback_msg->state.message = msg; + try + { + this->_goal_handle->publish_feedback(std::move(feedback_msg)); + return common::Result(); + } + catch (const std::runtime_error& e) + { + return e; + } +} + +} diff --git a/nexus_common/CMakeLists.txt b/nexus_common/CMakeLists.txt index ef46734..3701b90 100644 --- a/nexus_common/CMakeLists.txt +++ b/nexus_common/CMakeLists.txt @@ -118,17 +118,18 @@ if(BUILD_TESTING) endfunction() nexus_add_test(action_client_bt_node_test src/action_client_bt_node_test.cpp) - nexus_add_test(pausable_sequence_test src/pausable_sequence_test.cpp) - nexus_add_test(service_client_bt_node_test src/service_client_bt_node_test.cpp) + nexus_add_test(error_test src/main_test.cpp src/error_test.cpp) + nexus_add_test(logging_test src/logging_test.cpp) nexus_add_test(models_test src/main_test.cpp src/models/work_order_test.cpp ) + nexus_add_test(pausable_sequence_test src/pausable_sequence_test.cpp) nexus_add_test(ros_utils_test src/main_test.cpp src/batch_service_call_test.cpp ) - nexus_add_test(logging_test src/logging_test.cpp) + nexus_add_test(service_client_bt_node_test src/service_client_bt_node_test.cpp) nexus_add_test(sync_ros_client_test src/sync_ros_client_test.cpp) endif() diff --git a/nexus_common/include/nexus_common/action_client_bt_node.hpp b/nexus_common/include/nexus_common/action_client_bt_node.hpp index 4b38bc9..f65d74d 100644 --- a/nexus_common/include/nexus_common/action_client_bt_node.hpp +++ b/nexus_common/include/nexus_common/action_client_bt_node.hpp @@ -178,7 +178,16 @@ BT::NodeStatus ActionClientBtNode::onStart() RCLCPP_DEBUG( this->_node->get_logger(), "%s: Goal is accepted", this->name().c_str()); - this->_result_fut = this->_client->async_get_result(this->_goal_handle); + try + { + this->_result_fut = this->_client->async_get_result(this->_goal_handle); + } + catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) + { + RCLCPP_ERROR(this->_node->get_logger(), "%s: %s", + this->name().c_str(), e.what()); + return BT::NodeStatus::FAILURE; + } return BT::NodeStatus::RUNNING; } diff --git a/nexus_common/include/nexus_common/error.hpp b/nexus_common/include/nexus_common/error.hpp new file mode 100644 index 0000000..cc47979 --- /dev/null +++ b/nexus_common/include/nexus_common/error.hpp @@ -0,0 +1,129 @@ +/* + * 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_COMMON__ERROR_HPP +#define NEXUS_COMMON__ERROR_HPP + +#include +#include +#include +#include +#include + +namespace nexus::common { + +/** + * Represents the result of a function call that may return an error. + * + * Example usage: + * ```cpp + * const auto& result = foo(); + * if (result.error()) { + * // handle error + * } + * // use `result.value()` to get pointer to result value. + * ``` + */ +template +class Result +{ +public: Result(T t) + : _var(std::move(t)) {} + +public: Result(std::shared_ptr e) + : _var(std::move(e)) {} + +public: template>> + Result(D d) + : _var(std::make_shared(std::move(d))) {} + + /** + * Returns the value. + * @throws std::bad_variant_access if the result is an error. + */ +public: constexpr T& value() + { + return std::get(this->_var); + } + + /** + * Returns the value. + * @throws std::bad_variant_access if the result is an error. + */ +public: constexpr const T& value() const + { + return std::get(this->_var); + } + +public: constexpr E* error() + { + auto* maybe_err = std::get_if>(&this->_var); + return maybe_err ? maybe_err->get() : nullptr; + } + +public: constexpr const E* error() const + { + auto* maybe_err = std::get_if>(&this->_var); + return maybe_err ? maybe_err->get() : nullptr; + } + +private: std::variant> _var; +}; + +template +class Result +{ +public: Result() + : _err(std::nullopt) {} + +public: Result(std::shared_ptr e) + : _err(std::move(e)) {} + +public: template>> + Result(D d) + : _err(std::make_shared(std::move(d))) {} + + /** + * Throws if result is an error. + * @throws std::runtime_error if the result is an error. + */ +public: constexpr void value() const + { + if (this->_err.has_value()) + { + throw std::runtime_error("bad value access"); + } + } + +public: constexpr E* error() + { + return this->_err.has_value() ? this->_err->get() : nullptr; + } + +public: constexpr const E* error() const + { + return this->_err.has_value() ? this->_err->get() : nullptr; + } + +private: std::optional> _err; +}; + +} + +#endif diff --git a/nexus_common/src/error_test.cpp b/nexus_common/src/error_test.cpp new file mode 100644 index 0000000..4eba198 --- /dev/null +++ b/nexus_common/src/error_test.cpp @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2022 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 + +#include "nexus_common/error.hpp" + +namespace nexus::common::test { + +TEST_CASE("get_result_value_and_error") { + Result good(1); + CHECK(good.value() == 1); + CHECK(!good.error()); + + const Result const_good(1); + CHECK(const_good.value() == 1); + CHECK(!const_good.error()); + + Result bad(std::make_shared("bad")); + CHECK_THROWS(bad.value()); + CHECK(bad.error()); + + Result bad_val_constructor(std::runtime_error("bad_val_constructor")); + CHECK_THROWS(bad_val_constructor.value()); + CHECK(bad_val_constructor.error()); + + Result void_good; + CHECK(!void_good.error()); + + Result void_bad(std::make_shared("void_bad")); + CHECK(void_bad.error()); + + Result void_bad_val_constructor(std::runtime_error( + "void_bad_val_constructor")); + CHECK(void_bad_val_constructor.error()); +} + +} diff --git a/nexus_system_orchestrator/src/bid_transporter.cpp b/nexus_system_orchestrator/src/bid_transporter.cpp index af3dc25..7f5923e 100644 --- a/nexus_system_orchestrator/src/bid_transporter.cpp +++ b/nexus_system_orchestrator/src/bid_transporter.cpp @@ -27,20 +27,11 @@ BT::PortsList BidTransporter::providedPorts() BT::NodeStatus BidTransporter::onStart() { - auto node = this->_w_node.lock(); - if (!node) - { - std::cerr << - "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES" << - std::endl; - std::terminate(); - } - auto maybe_destination = this->getInput("destination"); if (!maybe_destination) { RCLCPP_ERROR( - node->get_logger(), "%s: [destination] param is required", + this->_node->get_logger(), "%s: [destination] param is required", this->name().c_str()); return BT::NodeStatus::FAILURE; } @@ -49,7 +40,7 @@ BT::NodeStatus BidTransporter::onStart() auto req = std::make_shared(); req->request.id = this->_ctx->job_id; - req->request.requester = node->get_name(); + req->request.requester = this->_node->get_name(); req->request.destination = destination; // send request to all transporters in parallel for (auto& [transporter_id, session] : this->_ctx->transporter_sessions) @@ -91,26 +82,17 @@ void BidTransporter::_cleanup_pending_requests() BT::NodeStatus BidTransporter::_update_ongoing_requests() { - auto node = this->_w_node.lock(); - if (!node) - { - std::cerr << - "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES" << - std::endl; - std::terminate(); - } - if (std::chrono::steady_clock::now() > this->_time_limit) { for (const auto& [transporter_id, _] : this->_ongoing_requests) { RCLCPP_WARN( - node->get_logger(), "%s: Skipped transporter [%s] (no response)", + this->_node->get_logger(), "%s: Skipped transporter [%s] (no response)", this->name().c_str(), transporter_id.c_str()); } RCLCPP_ERROR( - node->get_logger(), "%s: No transporter is able to perform task", + this->_node->get_logger(), "%s: No transporter is able to perform task", this->name().c_str()); return BT::NodeStatus::FAILURE; } @@ -128,7 +110,7 @@ BT::NodeStatus BidTransporter::_update_ongoing_requests() if (resp->available) { RCLCPP_INFO( - node->get_logger(), "[%s]: Bid awarded to [%s]", + this->_node->get_logger(), "[%s]: Bid awarded to [%s]", this->name().c_str(), transporter_id.c_str()); this->setOutput("result", transporter_id); @@ -138,7 +120,7 @@ BT::NodeStatus BidTransporter::_update_ongoing_requests() else { RCLCPP_DEBUG( - node->get_logger(), "%s: Transporter [%s] cannot perform task", + this->_node->get_logger(), "%s: Transporter [%s] cannot perform task", this->name().c_str(), transporter_id.c_str()); } } diff --git a/nexus_system_orchestrator/src/bid_transporter.hpp b/nexus_system_orchestrator/src/bid_transporter.hpp index b0e0bd1..86123e3 100644 --- a/nexus_system_orchestrator/src/bid_transporter.hpp +++ b/nexus_system_orchestrator/src/bid_transporter.hpp @@ -56,9 +56,9 @@ public: static BT::PortsList providedPorts(); public: BidTransporter(const std::string& name, const BT::NodeConfiguration& config, - rclcpp_lifecycle::LifecycleNode::WeakPtr w_node, + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr ctx) - : BT::StatefulActionNode{name, config}, _w_node{w_node}, _ctx{std::move(ctx)} + : BT::StatefulActionNode{name, config}, _node{node}, _ctx{std::move(ctx)} { } @@ -73,7 +73,7 @@ private: struct OngoingRequest FutureAndRequestId fut; }; -private: rclcpp_lifecycle::LifecycleNode::WeakPtr _w_node; +private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node; private: std::shared_ptr _ctx; private: std::unordered_map _ongoing_requests; private: std::chrono::steady_clock::time_point _time_limit; diff --git a/nexus_system_orchestrator/src/exceptions.hpp b/nexus_system_orchestrator/src/exit_code.hpp similarity index 73% rename from nexus_system_orchestrator/src/exceptions.hpp rename to nexus_system_orchestrator/src/exit_code.hpp index 7d60191..2d33d94 100644 --- a/nexus_system_orchestrator/src/exceptions.hpp +++ b/nexus_system_orchestrator/src/exit_code.hpp @@ -15,18 +15,14 @@ * */ -#ifndef NEXUS_SYSTEM_ORCHESTRATOR__EXCEPTIONS_HPP -#define NEXUS_SYSTEM_ORCHESTRATOR__EXCEPTIONS_HPP - -#include +#ifndef NEXUS_SYSTEM_ORCHESTRATOR__EXIT_CODE_HPP +#define NEXUS_SYSTEM_ORCHESTRATOR__EXIT_CODE_HPP namespace nexus::system_orchestrator { -class DuplicatedWorkOrderError : public std::runtime_error -{ -public: using std::runtime_error::runtime_error; -}; +constexpr int EXIT_CODE_INVALID_PARAMETER = 1; } + #endif diff --git a/nexus_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp index bd972e2..357ff5c 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.cpp +++ b/nexus_system_orchestrator/src/system_orchestrator.cpp @@ -19,8 +19,8 @@ #include "bid_transporter.hpp" #include "context.hpp" -#include "exceptions.hpp" #include "execute_task.hpp" +#include "exit_code.hpp" #include "for_each_task.hpp" #include "job.hpp" #include "send_signal.hpp" @@ -53,6 +53,11 @@ using WorkcellState = endpoints::WorkcellStateTopic::MessageType; using rcl_interfaces::msg::ParameterDescriptor; +class DuplicatedWorkOrderError : public std::runtime_error +{ +public: using std::runtime_error::runtime_error; +}; + /** * Error thrown when a work order cannot be completed. */ @@ -75,7 +80,8 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options) this->_bt_path = this->declare_parameter("bt_path", "", desc); if (this->_bt_path.empty()) { - throw std::runtime_error("param [bt_path] is required"); + RCLCPP_FATAL(this->get_logger(), "param [bt_path] is required"); + std::exit(EXIT_CODE_INVALID_PARAMETER); } // check if "main.xml" exists @@ -83,8 +89,10 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options) if (!std::filesystem::exists(main_bt) || !std::filesystem::is_regular_file(main_bt)) { - throw std::runtime_error( - "path specified in [bt_path] param must contain \"main.xml\""); + RCLCPP_FATAL( + this->get_logger(), + "path specified in [bt_path] param must contain \"main.xml\""); + std::exit(EXIT_CODE_INVALID_PARAMETER); } } @@ -170,25 +178,23 @@ auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous) std::shared_ptr goal) -> rclcpp_action::GoalResponse { - try + if (this->_max_parallel_jobs > 0 && + this->_jobs.size() >= static_cast(this->_max_parallel_jobs)) { - if (this->_max_parallel_jobs > 0 && - this->_jobs.size() >= static_cast(this->_max_parallel_jobs)) - { - auto result = std::make_shared(); - result->message = "Max number of parallel work orders reached"; - RCLCPP_ERROR(this->get_logger(), "%s", result->message.c_str()); - return rclcpp_action::GoalResponse::REJECT; - } - - this->_create_job(*goal); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + auto result = std::make_shared(); + result->message = "Max number of parallel work orders reached"; + RCLCPP_ERROR(this->get_logger(), "%s", result->message.c_str()); + return rclcpp_action::GoalResponse::REJECT; } - catch (const std::exception& e) + + const auto r = this->_create_job(*goal); + if (r.error()) { - RCLCPP_ERROR(this->get_logger(), "Failed to create job: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "Failed to create job: %s", + r.error()->what()); return rclcpp_action::GoalResponse::REJECT; } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, [this](const std::shared_ptr goal_handle) -> rclcpp_action::CancelResponse @@ -508,7 +514,8 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo, return bt_factory->createTreeFromFile(this->_bt_path / "main.xml"); } -void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal) +common::Result SystemOrchestrator::_create_job( + const WorkOrderActionType::Goal& goal) { auto wo = YAML::Load(goal.order.work_order).as(); @@ -530,8 +537,9 @@ void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal) auto result = std::make_shared(); result->message = "A work order with the same id is already running!"; RCLCPP_ERROR(this->get_logger(), "%s", result->message.c_str()); - throw DuplicatedWorkOrderError{result->message}; + return DuplicatedWorkOrderError(result->message); } + return common::Result(); } void SystemOrchestrator::_init_job( diff --git a/nexus_system_orchestrator/src/system_orchestrator.hpp b/nexus_system_orchestrator/src/system_orchestrator.hpp index 0cbc8a3..4cd8777 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.hpp +++ b/nexus_system_orchestrator/src/system_orchestrator.hpp @@ -22,6 +22,7 @@ #include "session.hpp" #include +#include #include #include #include @@ -110,7 +111,8 @@ class SystemOrchestrator : public * required contexts and start the job. * @throws std::exception */ - void _create_job(const WorkOrderActionType::Goal& goal); + [[nodiscard]] common::Result _create_job( + const WorkOrderActionType::Goal& goal); /** * Assigns all tasks and start the job associated with the goal handle. diff --git a/nexus_workcell_orchestrator/src/exceptions.hpp b/nexus_workcell_orchestrator/src/exceptions.hpp deleted file mode 100644 index b9be0ed..0000000 --- a/nexus_workcell_orchestrator/src/exceptions.hpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Copyright (C) 2022 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_WORKCELL_ORCHESTRATOR__EXECPTIONS_HPP -#define NEXUS_WORKCELL_ORCHESTRATOR__EXECPTIONS_HPP - -#include -#include - -namespace nexus::workcell_orchestrator { - -class DiscoveryError : public std::runtime_error -{ -public: DiscoveryError(const std::string& msg) - : std::runtime_error(msg) {} -}; - -class RegistrationError : public std::runtime_error -{ -public: int32_t error_code; - - /** - * Create new instance with message and error code. - * @param msg error message - * @param error_code error code as described in RegisterWorkcell or RegisterTransporter - */ -public: RegistrationError(const std::string& msg, int32_t error_code) - : std::runtime_error(msg), error_code(error_code) {} -}; - -} - -#endif diff --git a/nexus_capabilities/include/nexus_capabilities/exceptions.hpp b/nexus_workcell_orchestrator/src/exit_code.hpp similarity index 57% rename from nexus_capabilities/include/nexus_capabilities/exceptions.hpp rename to nexus_workcell_orchestrator/src/exit_code.hpp index 64100f6..459f86a 100644 --- a/nexus_capabilities/include/nexus_capabilities/exceptions.hpp +++ b/nexus_workcell_orchestrator/src/exit_code.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Johnson & Johnson + * 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. @@ -15,23 +15,14 @@ * */ -#ifndef NEXUS_CAPABILITIES__EXCEPTIONS_HPP -#define NEXUS_CAPABILITIES__EXCEPTIONS_HPP +#ifndef NEXUS_WORKCELL_ORCHESTRATOR__EXIT_CODE_HPP +#define NEXUS_WORKCELL_ORCHESTRATOR__EXIT_CODE_HPP -#include -#include +namespace nexus::workcell_orchestrator { -namespace nexus { - -/** - * Base class for all errors from capabilities. - */ -class CapabilityError : public std::runtime_error -{ -public: CapabilityError(const std::string& msg) - : std::runtime_error(msg) {} -}; +constexpr int EXIT_CODE_INVALID_PARAMETER = 1; +constexpr int EXIT_CODE_REGISTRATION_FAILED = 2; } -#endif // NEXUS_CAPABILITIES__EXCEPTIONS_HPP +#endif diff --git a/nexus_workcell_orchestrator/src/get_joint_constraints.cpp b/nexus_workcell_orchestrator/src/get_joint_constraints.cpp index 364ca11..c5dfe04 100644 --- a/nexus_workcell_orchestrator/src/get_joint_constraints.cpp +++ b/nexus_workcell_orchestrator/src/get_joint_constraints.cpp @@ -69,6 +69,8 @@ BT::NodeStatus GetJointConstraints::tick() } // Convert TrajectoryPoint to set of JointConstraints. + // `index` is checked above so the accesses here should never cause OOB. + // No need to catch exception. std::vector constraints; const auto& final_joint_values = joint_trajectory.points.at(index).positions; for (std::size_t i = 0; i < final_joint_values.size(); ++i) diff --git a/nexus_workcell_orchestrator/src/job_manager.cpp b/nexus_workcell_orchestrator/src/job_manager.cpp index c893b16..3058db9 100644 --- a/nexus_workcell_orchestrator/src/job_manager.cpp +++ b/nexus_workcell_orchestrator/src/job_manager.cpp @@ -76,10 +76,8 @@ void JobManager::_tick_bt(Job& job) } default: { std::ostringstream oss; - oss << "Error ticking task [" << job.ctx->task.id << - "]: Behavior tree returned invalid or unknown status [" << - bt_status << "]"; - RCLCPP_ERROR_STREAM(job.ctx->node->get_logger(), oss.str()); + oss << "behavior tree returned invalid or unknown status (" << + bt_status << ")"; throw std::runtime_error(oss.str()); } } @@ -120,7 +118,7 @@ uint8_t JobManager::_tick_job(Job& job) return task_status; } -Job& JobManager::assign_task( +common::Result JobManager::assign_task( const std::string& task_id) { const auto it = @@ -131,7 +129,11 @@ Job& JobManager::assign_task( }); if (it != this->_jobs.end()) { - throw JobError("Another task with the same id already exist"); + std::string msg("Another task with the same id already exist"); + RCLCPP_ERROR( + this->_node->get_logger(), "Failed to assign task [%s]: %s", + task_id.c_str(), msg.c_str()); + return JobError(msg); } auto& j = this->_jobs.emplace_back(Job{nullptr, std::nullopt, nullptr, @@ -149,10 +151,11 @@ Job& JobManager::assign_task( RCLCPP_INFO( this->_node->get_logger(), "Assigned task [%s]", task_id.c_str()); - return j; + return &j; } -Job& JobManager::queue_task(const GoalHandlePtr& goal_handle, +common::Result JobManager::queue_task( + const GoalHandlePtr& goal_handle, const std::shared_ptr& ctx, BT::Tree&& bt) { const auto& job_id = goal_handle->get_goal()->task.id; @@ -161,14 +164,15 @@ Job& JobManager::queue_task(const GoalHandlePtr& goal_handle, { return j.task_state.task_id == job_id; }); - if (it == this->_jobs.end()) + + if (it == this->_jobs.end() || + it->task_state.status != TaskState::STATUS_ASSIGNED) { - // abort the goal and throw error - const auto result = std::make_shared(); - result->success = false; - result->message = "Task not found"; - goal_handle->abort(result); - throw JobError("Task not found"); + std::string msg("Task is not assigned"); + RCLCPP_ERROR( + this->_node->get_logger(), "Failed to queue task [%s]: %s", + job_id.c_str(), msg.c_str()); + return JobError(msg); } it->ctx = ctx; @@ -180,10 +184,11 @@ Job& JobManager::queue_task(const GoalHandlePtr& goal_handle, RCLCPP_INFO( it->ctx->node->get_logger(), "Task [%s] is now queued", it->task_state.task_id.c_str()); - return *it; + return &*it; } -void JobManager::remove_assigned_task(const std::string& task_id) +common::Result JobManager::remove_assigned_task( + const std::string& task_id) { const auto it = std::find_if(this->_jobs.begin(), this->_jobs.end(), [&task_id](const Job& j) @@ -192,66 +197,112 @@ void JobManager::remove_assigned_task(const std::string& task_id) }); if (it == this->_jobs.end()) { - return; + return common::Result(); } if (it->task_state.status != TaskState::STATUS_ASSIGNED) { - throw JobError("Task is not assigned"); + std::string msg("Task is not assigned"); + RCLCPP_ERROR( + this->_node->get_logger(), "Failed to remove task [%s]: %s", + task_id.c_str(), msg.c_str()); + return JobError(msg); } this->_jobs.erase(it); + return common::Result(); } -void JobManager::halt_all_jobs() +common::Result JobManager::halt_all_jobs() { + const auto handle_exception = [this](const std::exception& e, const Job& j) + { + RCLCPP_ERROR( + this->_node->get_logger(), "Failed to halt job [%s]: %s", + j.task_state.task_id.c_str(), e.what()); + return e; + }; + RCLCPP_INFO(this->_node->get_logger(), "Halting all tasks"); for (auto& j : this->_jobs) { - // halt bts that are running - if (j.task_state.status == TaskState::STATUS_RUNNING) + try { - this->_ctx_mgr->set_active_context(j.ctx); - j.bt->haltTree(); - this->_ctx_mgr->set_active_context(nullptr); + // halt bts that are running + if (j.task_state.status == TaskState::STATUS_RUNNING) + { + this->_ctx_mgr->set_active_context(j.ctx); + j.bt->haltTree(); + this->_ctx_mgr->set_active_context(nullptr); - RCLCPP_INFO( - j.ctx->node->get_logger(), "Task [%s] halted", j.ctx->task.id.c_str()); - // Publish feedback. - j.task_state.status = TaskState::STATUS_FAILED; - auto fb = std::make_shared(); - fb->state = j.task_state; - j.goal_handle->publish_feedback(std::move(fb)); + RCLCPP_INFO( + j.ctx->node->get_logger(), "Task [%s] halted", + j.ctx->task.id.c_str()); + // Publish feedback. + j.task_state.status = TaskState::STATUS_FAILED; + auto fb = std::make_shared(); + fb->state = j.task_state; + j.goal_handle->publish_feedback(std::move(fb)); - // Abort the action request. - auto result = - std::make_shared(); - result->success = false; - result->message = "Task halted"; - j.goal_handle->abort(result); - break; + // Abort the action request. + auto result = + std::make_shared(); + result->success = false; + result->message = "Task halted"; + j.goal_handle->abort(result); + break; + } + } + catch (const rclcpp::exceptions::RCLError& e) + { + return handle_exception(e, j); + } + catch (const std::runtime_error& e) + { + return handle_exception(e, j); } } this->_jobs.clear(); + return common::Result(); } -void JobManager::tick() +common::Result JobManager::tick() { + const auto handle_exception = [this](const std::exception& e, const Job& j) + { + RCLCPP_ERROR( + this->_node->get_logger(), "Error ticking task [%s]: %s", + j.task_state.task_id.c_str(), e.what()); + return e; + }; + size_t i = 0; auto it = this->_jobs.begin(); for (; i < this->_max_concurrent && it != this->_jobs.end(); ++i) { - const auto task_status = this->_tick_job(*it); - if (task_status == TaskState::STATUS_FAILED || - task_status == TaskState::STATUS_FINISHED) + try + { + const auto task_status = this->_tick_job(*it); + if (task_status == TaskState::STATUS_FAILED || + task_status == TaskState::STATUS_FINISHED) + { + it = this->_jobs.erase(it); + continue; + } + else + { + ++it; + } + } + catch (const rclcpp::exceptions::RCLError& e) { - it = this->_jobs.erase(it); - continue; + return handle_exception(e, *it); } - else + catch (const std::runtime_error& e) { - ++it; + return handle_exception(e, *it); } } + return common::Result(); } } diff --git a/nexus_workcell_orchestrator/src/job_manager.hpp b/nexus_workcell_orchestrator/src/job_manager.hpp index d00209a..a2425f1 100644 --- a/nexus_workcell_orchestrator/src/job_manager.hpp +++ b/nexus_workcell_orchestrator/src/job_manager.hpp @@ -21,6 +21,7 @@ #include "job.hpp" #include +#include #include #include @@ -54,29 +55,28 @@ public: JobManager(rclcpp_lifecycle::LifecycleNode::SharedPtr node, /** * Assigns a task. - * @throw JobError */ -public: Job& assign_task(const std::string& task_id); +public: [[nodiscard]] common::Result assign_task( + const std::string& task_id); /** - * Queue a task. The task must already be assigned. If there is an error, the goal will - * immediately be aborted. - * @throw JobError + * Queue a task. The task must already be assigned. */ -public: Job& queue_task(const GoalHandlePtr& goal_handle, +public: [[nodiscard]] common::Result queue_task( + const GoalHandlePtr& goal_handle, const std::shared_ptr& ctx, BT::Tree&& bt); /** * Remove a assigned task. - * @throw JobError */ -public: void remove_assigned_task(const std::string& task_id); +public: [[nodiscard]] common::Result remove_assigned_task( + const std::string& task_id); /** * Forcefully stop and clear all jobs. Jobs will be stopped as soon as possible, unlike * `cancel_all_jobs`, the behavior trees cannot react to this event. */ -public: void halt_all_jobs(); +public: [[nodiscard]] common::Result halt_all_jobs(); /** * TODO: Implement on cancel bt node. @@ -88,7 +88,7 @@ public: void halt_all_jobs(); /** * Tick all active jobs. */ -public: void tick(); +public: [[nodiscard]] common::Result tick(); public: const std::list& jobs() const { diff --git a/nexus_workcell_orchestrator/src/job_manager_test.cpp b/nexus_workcell_orchestrator/src/job_manager_test.cpp index 5584b21..993449e 100644 --- a/nexus_workcell_orchestrator/src/job_manager_test.cpp +++ b/nexus_workcell_orchestrator/src/job_manager_test.cpp @@ -64,15 +64,15 @@ TEST_CASE("JobManager") { fixture.spin_in_background(); SECTION("cannot assign same task twice") { - CHECK_NOTHROW(job_mgr.assign_task("test")); - CHECK_THROWS_AS(job_mgr.assign_task("test"), JobError); + CHECK(job_mgr.assign_task("test").value()); + CHECK(job_mgr.assign_task("test").error()); } SECTION("cannot queue task that is not assigned") { handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle) { - CHECK_THROWS_AS(job_mgr.queue_task(goal_handle, nullptr, - bt_factory.createTreeFromText(bt)), JobError); + CHECK(job_mgr.queue_task(goal_handle, nullptr, + bt_factory.createTreeFromText(bt)).error()); }; WorkcellTask::Goal goal; @@ -91,7 +91,7 @@ TEST_CASE("JobManager") { SECTION("assign task sets job data correctly") { Job* job; - job = &job_mgr.assign_task("test"); + job = job_mgr.assign_task("test").value(); CHECK(!job->bt.has_value()); CHECK(!job->bt_logging); CHECK(job->ctx == nullptr); @@ -102,16 +102,16 @@ TEST_CASE("JobManager") { CHECK(job->tick_count == 0); SECTION("ticking assigned but unqueued task is a noop") { - job_mgr.tick(); + job_mgr.tick().value(); CHECK(job->tick_count == 0); } SECTION("can queue an assigned task") { handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle) { - auto ctx = std::make_shared(fixture.node); - CHECK(&job_mgr.queue_task(goal_handle, ctx, - bt_factory.createTreeFromText(bt)) == job); + auto ctx = std::make_shared(fixture.node, goal_handle); + CHECK(job_mgr.queue_task(goal_handle, ctx, + bt_factory.createTreeFromText(bt)).value() == job); CHECK(job->bt.has_value()); CHECK(job->bt_logging); REQUIRE(job->ctx == ctx); @@ -141,16 +141,16 @@ TEST_CASE("JobManager") { std::vector task_ids{"test1", "test2", "test3"}; for (const auto& task_id : task_ids) { - job_mgr.assign_task(task_id); + job_mgr.assign_task(task_id).value(); } std::promise done; size_t queued = 0; handle_accepted = [&](const JobManager::GoalHandlePtr& goal_handle) { - auto ctx = std::make_shared(fixture.node); - REQUIRE_NOTHROW(job_mgr.queue_task(goal_handle, ctx, - bt_factory.createTreeFromText(bt))); + auto ctx = std::make_shared(fixture.node, goal_handle); + REQUIRE(job_mgr.queue_task(goal_handle, ctx, + bt_factory.createTreeFromText(bt)).value()); if (++queued == task_ids.size()) { done.set_value(); @@ -168,11 +168,11 @@ TEST_CASE("JobManager") { REQUIRE(done.get_future().wait_for(std::chrono::seconds( 1)) == std::future_status::ready); - job_mgr.tick(); + job_mgr.tick().value(); // test1 and test2 should be finished CHECK(job_mgr.jobs().size() == 1); - job_mgr.tick(); + job_mgr.tick().value(); // test3 should now be finished CHECK(job_mgr.jobs().size() == 0); } diff --git a/nexus_workcell_orchestrator/src/task_parser.cpp b/nexus_workcell_orchestrator/src/task_parser.cpp index f3926b9..5cf0f3b 100644 --- a/nexus_workcell_orchestrator/src/task_parser.cpp +++ b/nexus_workcell_orchestrator/src/task_parser.cpp @@ -24,15 +24,22 @@ namespace nexus::workcell_orchestrator { //============================================================================== -Task TaskParser::parse_task( +common::Result TaskParser::parse_task( const nexus_orchestrator_msgs::msg::WorkcellTask& workcell_task) { - return Task{ - workcell_task.id, - this->remap_task_type(workcell_task.type), - YAML::Load(workcell_task.payload), - YAML::Load(workcell_task.previous_results), - }; + try + { + return Task{ + workcell_task.id, + this->remap_task_type(workcell_task.type), + YAML::Load(workcell_task.payload), + YAML::Load(workcell_task.previous_results), + }; + } + catch (const YAML::ParserException& e) + { + return e; + } } //============================================================================== diff --git a/nexus_workcell_orchestrator/src/task_parser.hpp b/nexus_workcell_orchestrator/src/task_parser.hpp index b93cf46..b473a2f 100644 --- a/nexus_workcell_orchestrator/src/task_parser.hpp +++ b/nexus_workcell_orchestrator/src/task_parser.hpp @@ -19,6 +19,7 @@ #define NEXUS_CAPABILITITY_PLUGIN__TASK_PARSER_HPP #include +#include #include #include @@ -34,9 +35,9 @@ public: TaskParser() {} /** * Parses a workcell task. - * @throws YAML::Exception if parsing the payload fails. */ -public: Task parse_task(const nexus_orchestrator_msgs::msg::WorkcellTask& task); +public: common::Result parse_task( + const nexus_orchestrator_msgs::msg::WorkcellTask& task); /** * Add task type to remap to. diff --git a/nexus_workcell_orchestrator/src/task_results_test.cpp b/nexus_workcell_orchestrator/src/task_results_test.cpp index 21b2b37..51503a8 100644 --- a/nexus_workcell_orchestrator/src/task_results_test.cpp +++ b/nexus_workcell_orchestrator/src/task_results_test.cpp @@ -34,7 +34,7 @@ namespace nexus::workcell_orchestrator::test { TEST_CASE("get and set results") { auto fixture = common::test::RosFixture{}; auto ctx_mgr = std::make_shared(); - auto ctx = std::make_shared(fixture.node); + auto ctx = std::make_shared(fixture.node, nullptr); ctx_mgr->set_active_context(ctx); BT::BehaviorTreeFactory bt_factory; diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp index 3512fb1..5092968 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -17,7 +17,7 @@ #include "workcell_orchestrator.hpp" -#include "exceptions.hpp" +#include "exit_code.hpp" #include "get_joint_constraints.hpp" #include "get_result.hpp" #include "make_transform.hpp" @@ -27,7 +27,6 @@ #include "transform_pose.hpp" #include -#include #include #include #include @@ -83,7 +82,8 @@ WorkcellOrchestrator::WorkcellOrchestrator(const rclcpp::NodeOptions& options) auto param = this->declare_parameter("bt_path", "", desc); if (param.empty()) { - throw std::runtime_error("param [bt_path] is required"); + RCLCPP_FATAL(this->get_logger(), "param [bt_path] is required"); + std::exit(EXIT_CODE_INVALID_PARAMETER); } this->_bt_path = std::move(param); } @@ -214,7 +214,7 @@ auto WorkcellOrchestrator::on_activate( RCLCPP_INFO(this->get_logger(), "Workcell activated"); this->_bt_timer = this->create_wall_timer(BT_TICK_RATE, [this]() { - this->_job_mgr.value().tick(); + this->_job_mgr.value().tick().value(); }); return CallbackReturn::SUCCESS; } @@ -224,7 +224,7 @@ auto WorkcellOrchestrator::on_deactivate( { RCLCPP_INFO(this->get_logger(), "Halting ongoing task before deactivating workcell"); - this->_job_mgr->halt_all_jobs(); + this->_job_mgr->halt_all_jobs().value(); this->_bt_timer->cancel(); this->_bt_timer.reset(); @@ -245,7 +245,7 @@ auto WorkcellOrchestrator::on_cleanup( this->_signal_wc_srv.reset(); this->_task_doable_srv.reset(); this->_cmd_server.reset(); - this->_job_mgr->halt_all_jobs(); + this->_job_mgr->halt_all_jobs().value(); RCLCPP_INFO(this->get_logger(), "Cleaned up"); return CallbackReturn::SUCCESS; } @@ -301,7 +301,7 @@ auto WorkcellOrchestrator::_configure( "Cancelling specific task is no longer supported, all tasks will be cancelled together to ensure line consistency"); } - this->_job_mgr->halt_all_jobs(); + this->_job_mgr->halt_all_jobs().value(); return rclcpp_action::CancelResponse::ACCEPT; }, [this](std::shared_ptr> @@ -316,17 +316,30 @@ auto WorkcellOrchestrator::_configure( return; } - auto ctx = std::make_shared(this->shared_from_this()); - ctx->task = this->_task_parser.parse_task(goal_handle->get_goal()->task); - auto bt = this->_create_bt(ctx); - try + auto ctx = std::make_shared( + this->shared_from_this(), goal_handle); + auto task_result = + this->_task_parser.parse_task(goal_handle->get_goal()->task); + if (task_result.error()) { - this->_job_mgr->queue_task(goal_handle, ctx, std::move(bt)); + auto result = std::make_shared(); + result->success = false; + result->message = task_result.error()->what(); + goal_handle->abort(result); + return; } - catch (const JobError& e) + ctx->task = task_result.value(); + + auto bt = this->_create_bt(ctx); + auto job_result = + this->_job_mgr->queue_task(goal_handle, ctx, std::move(bt)); + if (job_result.error()) { - RCLCPP_ERROR(this->get_logger(), "Failed to queue task [%s]: %s", - goal_handle->get_goal()->task.id.c_str(), e.what()); + auto result = std::make_shared(); + result->success = false; + result->message = job_result.error()->what(); + goal_handle->abort(result); + return; } }); @@ -380,18 +393,14 @@ auto WorkcellOrchestrator::_configure( ConstSharedPtr req, endpoints::QueueWorkcellTaskService::ServiceType::Response::SharedPtr resp) { - try - { - this->_job_mgr->assign_task(req->task_id); - resp->success = true; - } - catch (const JobError& e) + auto r = this->_job_mgr->assign_task(req->task_id); + if (r.error()) { resp->success = false; - resp->message = e.what(); - RCLCPP_ERROR(this->get_logger(), "Failed to assign task [%s]: %s", - req->task_id.c_str(), e.what()); + resp->message = r.error()->what(); + return; } + resp->success = true; }); this->_remove_pending_task_srv = @@ -403,19 +412,14 @@ auto WorkcellOrchestrator::_configure( { RCLCPP_DEBUG(this->get_logger(), "received request to remove pending task [%s]", req->task_id.c_str()); - try - { - this->_job_mgr->remove_assigned_task(req->task_id); - resp->success = true; - } - catch (const JobError& e) + auto r = this->_job_mgr->remove_assigned_task(req->task_id); + if (r.error()) { - RCLCPP_WARN(this->get_logger(), - "Failed to remove assigned task [%s]: %s", req->task_id.c_str(), - e.what()); resp->success = false; - resp->message = e.what(); + resp->message = r.error()->what(); + return; } + resp->success = true; }); this->_tf2_buffer = std::make_shared(this->get_clock()); @@ -560,30 +564,28 @@ auto WorkcellOrchestrator::_configure( } // configure capabilities - try + RCLCPP_INFO( + this->get_logger(), + "Configuring generic capabilities..." + ); + for (auto& [cap_id, cap] : this->_capabilities) { RCLCPP_INFO( this->get_logger(), - "Configuring generic capabilities..." + "configuring capability [%s]", + cap_id.c_str() ); - for (auto& [cap_id, cap] : this->_capabilities) + const auto r = cap->configure( + this->shared_from_this(), this->_job_mgr->context_manager(), + *_bt_factory); + if (r.error()) { - RCLCPP_INFO( - this->get_logger(), - "configuring capability [%s]", - cap_id.c_str() - ); - cap->configure( - this->shared_from_this(), this->_job_mgr->context_manager(), - *_bt_factory); + RCLCPP_ERROR( + this->get_logger(), "Failed to configure capability (%s)", + r.error()->what()); + return CallbackReturn::FAILURE; } } - catch (const CapabilityError& e) - { - RCLCPP_ERROR( - this->get_logger(), "Failed to configure capability (%s)", e.what()); - return CallbackReturn::FAILURE; - } // Create map for remapping capabilities auto remap_caps = this->get_parameter("remap_task_types").as_string(); @@ -658,7 +660,7 @@ void WorkcellOrchestrator::_register() RCLCPP_FATAL(this->get_logger(), "Failed to register with system orchestrator! [%s]", resp->message.c_str()); - throw RegistrationError(resp->message, resp->error_code); + std::exit(EXIT_CODE_REGISTRATION_FAILED); } return; } @@ -736,8 +738,13 @@ void WorkcellOrchestrator::_handle_task_doable( RCLCPP_DEBUG(this->get_logger(), "Got request to check task doable"); try { - auto task = this->_task_parser.parse_task(req->task); - resp->success = this->_can_perform_task(task); + auto r = this->_task_parser.parse_task(req->task); + if (r.error()) + { + resp->success = false; + return; + } + resp->success = this->_can_perform_task(r.value()); if (resp->success) { RCLCPP_DEBUG(this->get_logger(), "Workcell can perform task"); diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp index 12b8717..3bdade0 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -78,6 +79,8 @@ public: CallbackReturn on_deactivate( public: CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; +private: enum class ExiCode; + private: rclcpp_action::Server:: SharedPtr _cmd_server;