diff --git a/nexus_capabilities/src/capabilities/detection.cpp b/nexus_capabilities/src/capabilities/detection.cpp index fbe677e..3079ec7 100644 --- a/nexus_capabilities/src/capabilities/detection.cpp +++ b/nexus_capabilities/src/capabilities/detection.cpp @@ -95,7 +95,7 @@ bool DetectOffset::on_response( pose.pose = found_detection->bbox.center; RCLCPP_DEBUG_STREAM(this->_logger, *item << " is at " << pose.pose << " frame [" << pose.header.frame_id << - "]"); + "]"); this->setOutput("result", pose); return true; } @@ -171,7 +171,8 @@ BT::NodeStatus DetectAllItems::onRunning() if (d.id != *this->_cur_it) { RCLCPP_WARN( - this->_node->get_logger(), "detector requested to detect [%s] but it responded with [%s], ignoring the result", + this->_node->get_logger(), + "detector requested to detect [%s] but it responded with [%s], ignoring the result", this->_cur_it->c_str(), d.id.c_str()); continue; } diff --git a/nexus_capabilities/src/capabilities/detection_capability.cpp b/nexus_capabilities/src/capabilities/detection_capability.cpp index c546411..00a3052 100644 --- a/nexus_capabilities/src/capabilities/detection_capability.cpp +++ b/nexus_capabilities/src/capabilities/detection_capability.cpp @@ -63,15 +63,17 @@ void DetectionCapability::configure( auto node = w_node.lock(); if (!node) { - std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl; + 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) - { - return this->_clients.at(detector); - }); + node->get_logger(), ctx_mgr, [this, node](const std::string& detector) + { + return this->_clients.at(detector); + }); }); bt_factory.registerBuilder( @@ -82,15 +84,17 @@ void DetectionCapability::configure( auto node = w_node.lock(); if (!node) { - std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl; + 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) - { - return this->_clients.at(detector); - }); + [this](const std::string& detector) + { + return this->_clients.at(detector); + }); }); bt_factory.registerBuilder( diff --git a/nexus_capabilities/src/capabilities/dispense_item.hpp b/nexus_capabilities/src/capabilities/dispense_item.hpp index 298b7eb..3eaeff9 100644 --- a/nexus_capabilities/src/capabilities/dispense_item.hpp +++ b/nexus_capabilities/src/capabilities/dispense_item.hpp @@ -85,7 +85,8 @@ protected: endpoints::DispenserService::ServiceType::Request::SharedPtr make_request() final; protected: bool on_response( - rclcpp::Client::SharedResponse resp) + rclcpp::Client::SharedResponse + resp) final; private: std::vector _dispensers; diff --git a/nexus_capabilities/src/capabilities/dispense_item_capability.cpp b/nexus_capabilities/src/capabilities/dispense_item_capability.cpp index 1277d8c..0d5564e 100644 --- a/nexus_capabilities/src/capabilities/dispense_item_capability.cpp +++ b/nexus_capabilities/src/capabilities/dispense_item_capability.cpp @@ -49,7 +49,7 @@ void DispenseItemCapability::declare_params( //============================================================================== void DispenseItemCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::shared_ptr /* ctx_mgr */, + std::shared_ptr/* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) { auto dispenser_ids = node->get_parameter("dispensers").as_string_array(); @@ -74,7 +74,7 @@ void DispenseItemCapability::configure( { auto node = w_node.lock(); return std::make_unique(name, config, - node, this->_dispensers, std::chrono::milliseconds{dispenser_timeout}); + node, this->_dispensers, std::chrono::milliseconds{dispenser_timeout}); }); } diff --git a/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp b/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp index 8d66b25..90a9019 100644 --- a/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp +++ b/nexus_capabilities/src/capabilities/execute_trajectory_capability.cpp @@ -27,7 +27,7 @@ namespace nexus::capabilities { void ExecuteTrajectoryCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::shared_ptr /* ctx_mgr */, + std::shared_ptr/* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) { bt_factory.registerBuilder( @@ -38,7 +38,9 @@ void ExecuteTrajectoryCapability::configure( auto node = w_node.lock(); if (!node) { - std::cerr << "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" << std::endl; + std::cerr << + "FATAL ERROR!!! NODE IS DESTROYED WHILE THERE ARE STILL REFERENCES!!!" + << std::endl; std::terminate(); } diff --git a/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp b/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp index d544f5e..68fefd7 100644 --- a/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp +++ b/nexus_capabilities/src/capabilities/execute_trajectory_capability.hpp @@ -15,10 +15,8 @@ * */ -#ifndef \ - NEXUS_CAPABILITIES__CAPABILITIES__EXECUTE_TRAJECTORY_CAPABILITY_HPP -#define \ - NEXUS_CAPABILITIES__CAPABILITIES__EXECUTE_TRAJECTORY_CAPABILITY_HPP +#ifndef NEXUS_CAPABILITIES__CAPABILITIES__EXECUTE_TRAJECTORY_CAPABILITY_HPP +#define NEXUS_CAPABILITIES__CAPABILITIES__EXECUTE_TRAJECTORY_CAPABILITY_HPP #include #include diff --git a/nexus_capabilities/src/capabilities/gripper_capability.cpp b/nexus_capabilities/src/capabilities/gripper_capability.cpp index b4ae30d..9736bc7 100644 --- a/nexus_capabilities/src/capabilities/gripper_capability.cpp +++ b/nexus_capabilities/src/capabilities/gripper_capability.cpp @@ -33,7 +33,7 @@ void GripperCapability::declare_params(rclcpp_lifecycle::LifecycleNode& node) void GripperCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::shared_ptr /* ctx_mgr */, + std::shared_ptr/* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) { auto grippers = node->get_parameter("grippers").as_string_array(); diff --git a/nexus_capabilities/src/capabilities/plan_motion.cpp b/nexus_capabilities/src/capabilities/plan_motion.cpp index b762a93..f43ad59 100644 --- a/nexus_capabilities/src/capabilities/plan_motion.cpp +++ b/nexus_capabilities/src/capabilities/plan_motion.cpp @@ -133,14 +133,15 @@ make_request() RCLCPP_DEBUG_STREAM(this->_logger, "planning to " << req->goal_pose.pose << " at frame " << - req->goal_pose.header.frame_id << " with cartesian " << req->cartesian << "and scaled speed of " << - scale_speed); + req->goal_pose.header.frame_id << " with cartesian " << req->cartesian << + "and scaled speed of " << scale_speed); return req; } bool PlanMotion::on_response( - rclcpp::Client::SharedResponse resp) + rclcpp::Client::SharedResponse + resp) { if (resp->result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) diff --git a/nexus_capabilities/src/capabilities/plan_motion.hpp b/nexus_capabilities/src/capabilities/plan_motion.hpp index 57dcc58..d9a2549 100644 --- a/nexus_capabilities/src/capabilities/plan_motion.hpp +++ b/nexus_capabilities/src/capabilities/plan_motion.hpp @@ -82,7 +82,8 @@ public: static BT::PortsList providedPorts() public: PlanMotion(const std::string& name, const BT::NodeConfiguration& config, rclcpp_lifecycle::LifecycleNode& node, - rclcpp::Client::SharedPtr client, + rclcpp::Client::SharedPtr + client, std::shared_ptr tf_broadcaster) : ServiceClientBtNode(name, config, node.get_logger(), std::chrono::minutes{1}), _node{node}, @@ -100,7 +101,8 @@ protected: endpoints::GetMotionPlanService::ServiceType::Request::SharedPtr make_request() final; protected: bool on_response( - rclcpp::Client::SharedResponse resp) + rclcpp::Client::SharedResponse + resp) final; private: rclcpp_lifecycle::LifecycleNode& _node; diff --git a/nexus_capabilities/src/capabilities/plan_motion_capability.cpp b/nexus_capabilities/src/capabilities/plan_motion_capability.cpp index 7745047..075595f 100644 --- a/nexus_capabilities/src/capabilities/plan_motion_capability.cpp +++ b/nexus_capabilities/src/capabilities/plan_motion_capability.cpp @@ -29,7 +29,7 @@ namespace nexus::capabilities { void PlanMotionCapability::configure( rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::shared_ptr /* ctx_mgr */, + std::shared_ptr/* ctx_mgr */, BT::BehaviorTreeFactory& bt_factory) { this->_client = @@ -41,7 +41,7 @@ void PlanMotionCapability::configure( const BT::NodeConfiguration& config) { return std::make_unique(name, config, *node, this->_client, - tf_broadcaster); + tf_broadcaster); }); } 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..26ee456 100644 --- a/nexus_common/include/nexus_common/action_client_bt_node.hpp +++ b/nexus_common/include/nexus_common/action_client_bt_node.hpp @@ -79,7 +79,8 @@ protected: virtual void on_feedback(typename ActionT::Feedback::ConstSharedPtr); * the action is considered a success and false otherwise. */ protected: virtual bool on_result( - const typename rclcpp_action::ClientGoalHandle::WrappedResult& result); + const typename rclcpp_action::ClientGoalHandle::WrappedResult& + result); private: rclcpp::executors::SingleThreadedExecutor _executor; private: rclcpp::CallbackGroup::SharedPtr _cb_group; @@ -131,7 +132,7 @@ BT::NodeStatus ActionClientBtNode::onStart() typename rclcpp_action::Client::SendGoalOptions options; options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, - typename ActionT::Feedback::ConstSharedPtr feedback) + typename ActionT::Feedback::ConstSharedPtr feedback) { this->on_feedback(feedback); }; diff --git a/nexus_common/include/nexus_common/batch_service_call.hpp b/nexus_common/include/nexus_common/batch_service_call.hpp index c7fac57..45c3cb5 100644 --- a/nexus_common/include/nexus_common/batch_service_call.hpp +++ b/nexus_common/include/nexus_common/batch_service_call.hpp @@ -64,7 +64,7 @@ struct BatchServiceResult */ template>&)>> + BatchServiceResult>& )>> CancellationToken batch_service_call( NodePtrT node, const std::unordered_map>& batch_reqs, diff --git a/nexus_common/src/action_client_bt_node_test.cpp b/nexus_common/src/action_client_bt_node_test.cpp index d5ad96a..398876d 100644 --- a/nexus_common/src/action_client_bt_node_test.cpp +++ b/nexus_common/src/action_client_bt_node_test.cpp @@ -44,7 +44,8 @@ public: static BT::PortsList providedPorts() public: TestActionBtNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node::SharedPtr node, - rclcpp_action::Client::SharedPtr client) + rclcpp_action::Client::SharedPtr + client) : ActionClientBtNode(name, config, node), _client(client) {} @@ -116,13 +117,15 @@ TEST_CASE("ActionClientBtNode", "[BehaviorTree]") return rclcpp_action::GoalResponse::REJECT; }, [&]( - std::shared_ptr>) + std::shared_ptr>) { rejected_goal.set_value(); return rclcpp_action::CancelResponse::REJECT; }, []( - std::shared_ptr> + std::shared_ptr> handle) { handle->abort( @@ -145,12 +148,14 @@ TEST_CASE("ActionClientBtNode", "[BehaviorTree]") return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, []( - std::shared_ptr>) + std::shared_ptr>) { return rclcpp_action::CancelResponse::REJECT; }, []( - std::shared_ptr> + std::shared_ptr> handle) { handle->abort( @@ -178,13 +183,15 @@ TEST_CASE("ActionClientBtNode", "[BehaviorTree]") return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, [&]( - std::shared_ptr>) + std::shared_ptr>) { got_cancel.set_value(); return rclcpp_action::CancelResponse::ACCEPT; }, [&]( - std::shared_ptr> + std::shared_ptr> handle) { started_job.set_value(); @@ -202,7 +209,7 @@ TEST_CASE("ActionClientBtNode", "[BehaviorTree]") tick_until(bt, [&](auto) { return started_job_fut.wait_for(std::chrono::seconds( - 0)) == std::future_status::ready; + 0)) == std::future_status::ready; }, std::chrono::seconds(1)); bt.haltTree(); CHECK(got_cancel_fut.wait_for(std::chrono::seconds(0)) == @@ -225,12 +232,14 @@ TEST_CASE("ActionClientBtNode", "[BehaviorTree]") return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, [&]( - std::shared_ptr>) + std::shared_ptr>) { return rclcpp_action::CancelResponse::REJECT; }, [&]( - std::shared_ptr> + std::shared_ptr> handle) { started_job.set_value(); @@ -247,7 +256,7 @@ TEST_CASE("ActionClientBtNode", "[BehaviorTree]") tick_until(bt, [&](auto) { return started_job_fut.wait_for(std::chrono::seconds( - 0)) == std::future_status::ready; + 0)) == std::future_status::ready; }, std::chrono::seconds(1)); // test action takes 1 second, halting the tree should cause it to block until @@ -270,12 +279,14 @@ TEST_CASE("ActionClientBtNode", "[BehaviorTree]") return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, []( - std::shared_ptr>) + std::shared_ptr>) { return rclcpp_action::CancelResponse::REJECT; }, []( - std::shared_ptr> + std::shared_ptr> handle) { handle->succeed( diff --git a/nexus_common/src/models/work_order_test.cpp b/nexus_common/src/models/work_order_test.cpp index aa3d2be..4270faa 100644 --- a/nexus_common/src/models/work_order_test.cpp +++ b/nexus_common/src/models/work_order_test.cpp @@ -59,7 +59,8 @@ TEST_CASE("WorkOrder serialization", "[Model][Serialization]") } )RAW"}; - auto check_data = [](const WorkOrder& work_order) + auto check_data = + [](const WorkOrder& work_order) { CHECK(work_order.number() == "SO/2022/20/1-1"); CHECK(work_order.work_instruction_name() == "CV-299 (Rev 4)"); diff --git a/nexus_common/src/node_utils.cpp b/nexus_common/src/node_utils.cpp index 33c26df..ddc995e 100644 --- a/nexus_common/src/node_utils.cpp +++ b/nexus_common/src/node_utils.cpp @@ -102,7 +102,8 @@ get_node_options_default(bool allow_undeclared, bool declare_initial_params) { rclcpp::NodeOptions options; options.allow_undeclared_parameters(allow_undeclared); - options.automatically_declare_parameters_from_overrides(declare_initial_params); + options.automatically_declare_parameters_from_overrides( + declare_initial_params); return options; } diff --git a/nexus_gazebo/include/nexus_gazebo/Components.hh b/nexus_gazebo/include/nexus_gazebo/Components.hh index f26aa29..f4afd2e 100644 --- a/nexus_gazebo/include/nexus_gazebo/Components.hh +++ b/nexus_gazebo/include/nexus_gazebo/Components.hh @@ -25,7 +25,7 @@ namespace nexus_gazebo::components { using MotionCaptureRigidBody = gz::sim::components::Component< std::string, class MotionCaptureRigidBodyTag, - gz::sim::serializers::StringSerializer>; + gz::sim::serializers::StringSerializer>; GZ_SIM_REGISTER_COMPONENT( "nexus_gazebo.components.MotionCaptureRigidBody", diff --git a/nexus_integration_tests/src/mock_transporter.cpp b/nexus_integration_tests/src/mock_transporter.cpp index 512e507..7b4f1b1 100644 --- a/nexus_integration_tests/src/mock_transporter.cpp +++ b/nexus_integration_tests/src/mock_transporter.cpp @@ -269,8 +269,8 @@ class MockTransporter : public Transporter state.location.pose.position.x += x_increment; dist_traveled += abs(x_increment); - _transporters.at( - current_transporter)->current_location.pose = state.location.pose.position.x; + _transporters.at(current_transporter)->current_location.pose = + state.location.pose.position.x; feedback_cb(state); } diff --git a/nexus_integration_tests/src/test_mock_gripper.cpp b/nexus_integration_tests/src/test_mock_gripper.cpp index 68206f2..adea2a5 100644 --- a/nexus_integration_tests/src/test_mock_gripper.cpp +++ b/nexus_integration_tests/src/test_mock_gripper.cpp @@ -65,8 +65,7 @@ SCENARIO("Test Gripper Mock") double end_position = 0.0; auto goal_response_callback = - [&goal_response](GoalHandleGripperAction::SharedPtr - goal_handle) + [&goal_response](GoalHandleGripperAction::SharedPtr goal_handle) { if (goal_handle) { diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index 1c07a7a..bb9a4a9 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -106,7 +106,8 @@ class Implementation void message(const std::string& msg) { RCLCPP_INFO( - this->node_->get_logger(), ANSI_COLOR_BLUE "\33[1m%s\33[0m" ANSI_COLOR_RESET, + this->node_->get_logger(), + ANSI_COLOR_BLUE "\33[1m%s\33[0m" ANSI_COLOR_RESET, msg.c_str()); } @@ -127,8 +128,8 @@ class Implementation } void isActiveCallback( - const std::shared_ptr /*request_header*/, - const std::shared_ptr /*request*/, + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, std::shared_ptr response) { response->success = this->_system_active; diff --git a/nexus_lifecycle_manager/test/test_lifecycle_manager.cpp b/nexus_lifecycle_manager/test/test_lifecycle_manager.cpp index 2709e68..d185cd8 100644 --- a/nexus_lifecycle_manager/test/test_lifecycle_manager.cpp +++ b/nexus_lifecycle_manager/test/test_lifecycle_manager.cpp @@ -110,11 +110,10 @@ bool wait_for_service_by_node( [node, node_name, service]() { - auto service_names_and_types_by_node = node->get_service_names_and_types_by_node( - node_name, - ""); - return service_names_and_types_by_node.end() != service_names_and_types_by_node.find( - service); + auto service_names_and_types_by_node = + node->get_service_names_and_types_by_node(node_name, ""); + return service_names_and_types_by_node.end() != + service_names_and_types_by_node.find(service); }, timeout, sleep_period); @@ -233,7 +232,8 @@ SCENARIO("Test Lifecycle Manager") node3->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); - node3->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + node3->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); CHECK(lifecycle_manager->addNodeName("node3")); RCLCPP_INFO(node->get_logger(), "Added node 3"); diff --git a/nexus_motion_planner/src/test_motion_plan_cache.cpp b/nexus_motion_planner/src/test_motion_plan_cache.cpp index 64e71c7..e787152 100644 --- a/nexus_motion_planner/src/test_motion_plan_cache.cpp +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -44,7 +44,8 @@ void check_and_emit( moveit_msgs::msg::RobotTrajectory get_dummy_panda_plan() { - static moveit_msgs::msg::RobotTrajectory out = []() + static moveit_msgs::msg::RobotTrajectory out = + []() { moveit_msgs::msg::RobotTrajectory plan; @@ -72,7 +73,8 @@ moveit_msgs::msg::RobotTrajectory get_dummy_panda_plan() std::vector get_dummy_waypoints() { - static std::vector out = []() + static std::vector out = + []() { std::vector waypoints; for (size_t i = 0; i < 3; i++) @@ -1314,4 +1316,4 @@ int main(int argc, char** argv) // CARTESIAN PLANS === // Check construct plan request -// Remember to check fraction too! \ No newline at end of file +// Remember to check fraction too! diff --git a/nexus_robot_controller/src/robot_controller_server.cpp b/nexus_robot_controller/src/robot_controller_server.cpp index 8277e56..c6e1391 100644 --- a/nexus_robot_controller/src/robot_controller_server.cpp +++ b/nexus_robot_controller/src/robot_controller_server.cpp @@ -103,11 +103,11 @@ RobotControllerServer::on_configure(const rclcpp_lifecycle::State& /*state*/) get_logger(), "ROBOT_DESCRIPTION PARAM VALUE:" << node_->get_parameter("robot_description").as_string() - ); + ); if (node_->get_parameter("robot_description").as_string().empty()) { RCLCPP_WARN_STREAM(get_logger(), pimpl_->server_logging_prefix_.str() - << "Failed to configure. Missing robot_description parameter!"); + << "Failed to configure. Missing robot_description parameter!"); return CallbackReturn::FAILURE; } @@ -135,7 +135,7 @@ RobotControllerServer::on_configure(const rclcpp_lifecycle::State& /*state*/) pimpl_->cm_node_->get_logger(), cm_logging_prefix_.str() << "'update_rate' parameter not set, using default value." - ); + ); } RCLCPP_INFO_STREAM( pimpl_->cm_node_->get_logger(), @@ -196,7 +196,8 @@ RobotControllerServer::on_configure(const rclcpp_lifecycle::State& /*state*/) { std::string params_file; node_->get_parameter("ros2_control_params_file", params_file); - pimpl_->cm_node_->set_parameter({controller_name + ".params_file", params_file}); + pimpl_->cm_node_->set_parameter({controller_name + ".params_file", + params_file}); pimpl_->cm_node_->load_controller(controller_name); pimpl_->cm_node_->configure_controller(controller_name); } @@ -306,7 +307,7 @@ RobotControllerServer::on_shutdown(const rclcpp_lifecycle::State& state) pimpl_->server_logging_prefix_.str() << "Shutting Down"); RCLCPP_INFO_STREAM(get_logger(), pimpl_->server_logging_prefix_.str() << - "Shutdown: Running deactivate and cleanup"); + "Shutdown: Running deactivate and cleanup"); on_deactivate(state); on_cleanup(state); diff --git a/nexus_rviz_plugins/include/nexus_rviz_plugins/nexus_panel.hpp b/nexus_rviz_plugins/include/nexus_rviz_plugins/nexus_panel.hpp index 60eb012..f3f66fb 100644 --- a/nexus_rviz_plugins/include/nexus_rviz_plugins/nexus_panel.hpp +++ b/nexus_rviz_plugins/include/nexus_rviz_plugins/nexus_panel.hpp @@ -142,7 +142,8 @@ class InitialThread : public QThread Q_OBJECT public: - InitialThread(std::shared_ptr& + InitialThread( + std::shared_ptr& _client_orchestrator) : client_orchestrator_(_client_orchestrator) { @@ -161,8 +162,8 @@ class InitialThread : public QThread if (status_orchestrator == nexus::lifecycle_manager::SystemStatus::TIMEOUT) { - status_orchestrator = client_orchestrator_->is_active(std::chrono::seconds( - 1)); + status_orchestrator = + client_orchestrator_->is_active(std::chrono::seconds(1)); } } diff --git a/nexus_system_orchestrator/src/execute_task.cpp b/nexus_system_orchestrator/src/execute_task.cpp index fbf626a..289a476 100644 --- a/nexus_system_orchestrator/src/execute_task.cpp +++ b/nexus_system_orchestrator/src/execute_task.cpp @@ -56,7 +56,8 @@ BT::NodeStatus ExecuteTask::onStart() 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->_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; } diff --git a/nexus_system_orchestrator/src/execute_task.hpp b/nexus_system_orchestrator/src/execute_task.hpp index c3ce603..f4dbf28 100644 --- a/nexus_system_orchestrator/src/execute_task.hpp +++ b/nexus_system_orchestrator/src/execute_task.hpp @@ -54,8 +54,9 @@ public: ExecuteTask(const std::string& name, std::shared_ptr ctx, std::filesystem::path bt_path, std::shared_ptr bt_factory) - : BT::StatefulActionNode(name, config), _ctx(std::move(ctx)), _bt_path(std::move( - bt_path)), + : 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; diff --git a/nexus_system_orchestrator/src/send_signal.cpp b/nexus_system_orchestrator/src/send_signal.cpp index 8426974..ba935c2 100644 --- a/nexus_system_orchestrator/src/send_signal.cpp +++ b/nexus_system_orchestrator/src/send_signal.cpp @@ -54,7 +54,8 @@ BT::NodeStatus SendSignal::tick() if (it == this->_ctx->workcell_task_assignments.cend()) { RCLCPP_ERROR( - this->_ctx->node.get_logger(), "%s: Unable to find workcell assigned to task [%s]", + this->_ctx->node.get_logger(), + "%s: Unable to find workcell assigned to task [%s]", this->name().c_str(), task->id.c_str()); return BT::NodeStatus::FAILURE; } diff --git a/nexus_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp index bd972e2..e3c4fa7 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.cpp +++ b/nexus_system_orchestrator/src/system_orchestrator.cpp @@ -294,7 +294,8 @@ auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous) for (auto& p : this->_workcell_sessions) { auto& wc = p.second; - auto wc_req = std::make_shared(); + auto wc_req = + std::make_shared(); wc_req->pause = req->pause; reqs.emplace(p.first, common::BatchServiceReq{ @@ -453,27 +454,27 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo, [this, ctx](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, *this, ctx, - [this, ctx](const auto&) - { - this->_publish_wo_feedback(*ctx); - try - { - this->_publish_wo_states(this->_jobs.at(ctx->job_id)); - } - catch (const std::out_of_range&) + [this, ctx](const auto&) { - RCLCPP_WARN(this->get_logger(), - "Error when publishing work order states: Work order [%s] not found", - ctx->job_id.c_str()); - } - }); + this->_publish_wo_feedback(*ctx); + try + { + this->_publish_wo_states(this->_jobs.at(ctx->job_id)); + } + catch (const std::out_of_range&) + { + RCLCPP_WARN(this->get_logger(), + "Error when publishing work order states: Work order [%s] not found", + ctx->job_id.c_str()); + } + }); }); bt_factory->registerBuilder("BidTransporter", [this, ctx](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, - this->shared_from_this(), ctx); + this->shared_from_this(), ctx); }); bt_factory->registerBuilder( @@ -488,7 +489,7 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo, const BT::NodeConfiguration& config) { return std::make_unique(name, config, - this->get_logger(), ctx); + this->get_logger(), ctx); }); bt_factory->registerBuilder("ExecuteTask", @@ -496,7 +497,7 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo, const BT::NodeConfiguration& config) { return std::make_unique(name, config, ctx, this->_bt_path, - bt_factory); + bt_factory); }); bt_factory->registerBuilder("SendSignal", @@ -592,7 +593,8 @@ void SystemOrchestrator::_init_job( task_state.workcell_id = *maybe_assignment; task_state.task_id = task_id; auto req = - std::make_shared(); + std::make_shared(); req->task_id = task_id; try { @@ -730,16 +732,17 @@ void SystemOrchestrator::_handle_register_workcell( req->description, std::move(state), this->create_client( - endpoints::IsTaskDoableService::service_name( - workcell_id)), + endpoints::IsTaskDoableService::service_name(workcell_id)), this->create_client( - endpoints:: - PauseWorkcellService::service_name(workcell_id)), - std::make_unique>( + endpoints::PauseWorkcellService::service_name(workcell_id)), + std::make_unique>( this, endpoints::SignalWorkcellService::service_name(workcell_id)), - std::make_unique>( + std::make_unique>( this, endpoints::QueueWorkcellTaskService::service_name(workcell_id)), - std::make_unique>( + std::make_unique>( this, endpoints::RemovePendingTaskService::service_name(workcell_id)) })); @@ -760,7 +763,8 @@ void SystemOrchestrator::_handle_register_workcell( } void SystemOrchestrator::_handle_register_transporter( - endpoints::RegisterTransporterService::ServiceType::Request::ConstSharedPtr req, + endpoints::RegisterTransporterService::ServiceType::Request::ConstSharedPtr + req, endpoints::RegisterTransporterService::ServiceType::Response::SharedPtr resp) { RCLCPP_DEBUG(this->get_logger(), "Got register transporter request"); @@ -780,7 +784,8 @@ void SystemOrchestrator::_handle_register_transporter( this->_transporter_sessions.emplace(transporter_id, std::make_shared(TransporterSession{ req->description, - this->create_client( + this->create_client + ( endpoints::IsTransporterAvailableService::service_name( transporter_id)) })); @@ -812,7 +817,8 @@ void SystemOrchestrator::_halt_job(const std::string& job_id) catch (const std::out_of_range&) { RCLCPP_WARN( - this->get_logger(), "Failed to remove pending task [%s]: missing task state", + this->get_logger(), + "Failed to remove pending task [%s]: missing task state", task_id.c_str()); } @@ -852,7 +858,8 @@ void SystemOrchestrator::_halt_fail_and_remove_job(const std::string& job_id) catch (const std::out_of_range&) { RCLCPP_WARN( - this->get_logger(), "Failed to halt, fail and remove job [%s]: job does not exist", + this->get_logger(), + "Failed to halt, fail and remove job [%s]: job does not exist", job_id.c_str()); } } diff --git a/nexus_system_orchestrator/src/system_orchestrator.hpp b/nexus_system_orchestrator/src/system_orchestrator.hpp index 0cbc8a3..9831c35 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.hpp +++ b/nexus_system_orchestrator/src/system_orchestrator.hpp @@ -55,7 +55,8 @@ class SystemOrchestrator : public using WorkcellTask = nexus_orchestrator_msgs::msg::WorkcellTask; using WorkOrderState = nexus_orchestrator_msgs::msg::WorkOrderState; - SystemOrchestrator(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + SystemOrchestrator( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); CallbackReturn on_configure(const rclcpp_lifecycle::State& previous) override; @@ -132,7 +133,8 @@ class SystemOrchestrator : public endpoints::RegisterWorkcellService::ServiceType::Response::SharedPtr resp); void _handle_register_transporter( - endpoints::RegisterTransporterService::ServiceType::Request::ConstSharedPtr req, + endpoints::RegisterTransporterService::ServiceType::Request::ConstSharedPtr + req, endpoints::RegisterTransporterService::ServiceType::Response::SharedPtr resp); /** diff --git a/nexus_system_orchestrator/src/workcell_request.cpp b/nexus_system_orchestrator/src/workcell_request.cpp index e53bec4..b9ae31f 100644 --- a/nexus_system_orchestrator/src/workcell_request.cpp +++ b/nexus_system_orchestrator/src/workcell_request.cpp @@ -94,8 +94,9 @@ WorkcellRequest::make_goal() } goal.task.previous_results = this->_ctx->task_results; RCLCPP_INFO_STREAM( - this->_ctx->node.get_logger(), "%s: Sending workcell request:" << std::endl << nexus_orchestrator_msgs::action::to_yaml( - goal)); + this->_ctx->node.get_logger(), + "%s: Sending workcell request:" << std::endl << + nexus_orchestrator_msgs::action::to_yaml(goal)); return goal; } @@ -107,7 +108,8 @@ void WorkcellRequest::on_feedback( } bool WorkcellRequest::on_result( - const rclcpp_action::ClientGoalHandle::WrappedResult& result) + const rclcpp_action::ClientGoalHandle::WrappedResult& result) { if (!result.result->success) { diff --git a/nexus_system_orchestrator/src/workcell_request.hpp b/nexus_system_orchestrator/src/workcell_request.hpp index 2b06608..a83d99d 100644 --- a/nexus_system_orchestrator/src/workcell_request.hpp +++ b/nexus_system_orchestrator/src/workcell_request.hpp @@ -56,8 +56,9 @@ public: inline WorkcellRequest(const std::string& name, rclcpp_lifecycle::LifecycleNode& node, std::shared_ptr ctx, OnTaskProgressCb on_task_progress_cb) : common::ActionClientBtNode(name, config, &node), _ctx(std::move( - ctx)), _on_task_progress(std::move(on_task_progress_cb)) {} + endpoints::WorkcellRequestAction::ActionType>(name, config, &node), + _ctx(std::move(ctx)), + _on_task_progress(std::move(on_task_progress_cb)) {} public: BT::NodeStatus onStart() override; @@ -71,7 +72,8 @@ protected: void on_feedback( override; protected: bool on_result( - const rclcpp_action::ClientGoalHandle::WrappedResult& result) + const rclcpp_action::ClientGoalHandle::WrappedResult& result) override; private: Task _task; diff --git a/nexus_workcell_orchestrator/src/job.cpp b/nexus_workcell_orchestrator/src/job.cpp index 8dd981f..bad8914 100644 --- a/nexus_workcell_orchestrator/src/job.cpp +++ b/nexus_workcell_orchestrator/src/job.cpp @@ -54,7 +54,7 @@ Job::Job(rclcpp_lifecycle::LifecycleNode::WeakPtr node, [this](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, this->_ctx, - *this->_node.lock()); + *this->_node.lock()); }); this->_bt_factory.registerNodeType("MakeTransform"); @@ -63,8 +63,8 @@ Job::Job(rclcpp_lifecycle::LifecycleNode::WeakPtr node, [this](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, - *this->_node.lock(), - this->_tf2_buffer); + *this->_node.lock(), + this->_tf2_buffer); }); this->_bt_factory.registerBuilder( @@ -72,7 +72,7 @@ Job::Job(rclcpp_lifecycle::LifecycleNode::WeakPtr node, [this](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, - *this->_node.lock()); + *this->_node.lock()); }); this->_bt_factory.registerBuilder( "DeserializeDetections", @@ -80,7 +80,7 @@ Job::Job(rclcpp_lifecycle::LifecycleNode::WeakPtr node, const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, - *this->_node.lock()); + *this->_node.lock()); }); this->_bt_factory.registerBuilder("WaitForSignal", diff --git a/nexus_workcell_orchestrator/src/serialization_test.cpp b/nexus_workcell_orchestrator/src/serialization_test.cpp index 2eab72e..2b59068 100644 --- a/nexus_workcell_orchestrator/src/serialization_test.cpp +++ b/nexus_workcell_orchestrator/src/serialization_test.cpp @@ -41,7 +41,7 @@ TEST_CASE("pose serialization") { [&](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, - *fixture.node); + *fixture.node); }); auto bt = bt_factory.createTreeFromText( diff --git a/nexus_workcell_orchestrator/src/transform_pose.cpp b/nexus_workcell_orchestrator/src/transform_pose.cpp index 32195df..f617b3c 100644 --- a/nexus_workcell_orchestrator/src/transform_pose.cpp +++ b/nexus_workcell_orchestrator/src/transform_pose.cpp @@ -63,7 +63,8 @@ BT::NodeStatus ApplyTransform::tick() if (!x || !y || !z || !qx || !qy || !qz || !qw) { RCLCPP_ERROR( - this->_node.get_logger(), "[%s]: Either [transform_from_pose_stamped], [transform_from_pose], or [x, y, z, qx, qy, qz, qw] ports are required.", + this->_node.get_logger(), + "[%s]: Either [transform_from_pose_stamped], [transform_from_pose], or [x, y, z, qx, qy, qz, qw] ports are required.", this->name().c_str()); return BT::NodeStatus::FAILURE; } @@ -94,10 +95,12 @@ BT::NodeStatus ApplyTransform::tick() RCLCPP_INFO_STREAM( this->_node.get_logger(), "[" << this->name() << "]" << std::endl << - "Base pose: " << base_pose->pose << " frame: " << base_pose->header.frame_id << std::endl << + "Base pose: " << base_pose->pose << " frame: " << + base_pose->header.frame_id << std::endl << "Transform: " << t << std::endl << - "Result: " << result_pose.pose << " frame: " << result_pose.header.frame_id - ); + "Result: " << result_pose.pose << " frame: " << + result_pose.header.frame_id + ); this->setOutput("result", result_pose); return BT::NodeStatus::SUCCESS; } @@ -146,7 +149,8 @@ BT::NodeStatus GetTransform::tick() } tf2::Transform frame_tf; - const auto handle_exception = [this](const std::exception& e) + const auto handle_exception = + [this](const std::exception& e) { RCLCPP_ERROR(this->_node.get_logger(), "%s", e.what()); return BT::NodeStatus::FAILURE; @@ -188,8 +192,10 @@ BT::NodeStatus GetTransform::tick() RCLCPP_INFO_STREAM( this->_node.get_logger(), "[" << this->name() << "]" << std::endl << - "Base pose: " << base_pose->pose << " frame: " << base_pose->header.frame_id << std::endl << - "Target pose: " << target_pose->pose << "frame: " << target_pose->header.frame_id << std::endl << + "Base pose: " << base_pose->pose << " frame: " << + base_pose->header.frame_id << std::endl << + "Target pose: " << target_pose->pose << "frame: " << + target_pose->header.frame_id << std::endl << "Result: " << result_tf; ); this->setOutput("result", tf2::toMsg(result_tf)); diff --git a/nexus_workcell_orchestrator/src/transform_pose_test.cpp b/nexus_workcell_orchestrator/src/transform_pose_test.cpp index ae40807..06ff48d 100644 --- a/nexus_workcell_orchestrator/src/transform_pose_test.cpp +++ b/nexus_workcell_orchestrator/src/transform_pose_test.cpp @@ -148,7 +148,7 @@ TEST_CASE("GetTransform") { [&](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, *fixture.node, - tf2_buffer); + tf2_buffer); }); auto bt = factory.createTreeFromText( diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp index 63adf07..ca9374f 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -288,7 +288,8 @@ auto WorkcellOrchestrator::_configure( } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [this](std::shared_ptr> + [this](std::shared_ptr> goal_handle) { RCLCPP_DEBUG(this->get_logger(), "Got cancel request"); @@ -315,7 +316,8 @@ auto WorkcellOrchestrator::_configure( } return rclcpp_action::CancelResponse::ACCEPT; }, - [this](std::shared_ptr> + [this](std::shared_ptr> goal_handle) { if (this->get_current_state().label() != "active") @@ -350,7 +352,8 @@ auto WorkcellOrchestrator::_configure( if (!this->_can_perform_task( ctx->task)) { - auto result = std::make_shared(); + auto result = + std::make_shared(); result->message = "Workcell cannot perform task " + ctx->task.type; result->success = false; RCLCPP_ERROR_STREAM(this->get_logger(), result->message); @@ -367,7 +370,8 @@ auto WorkcellOrchestrator::_configure( catch (const std::exception& e) { std::ostringstream oss; - auto result = std::make_shared(); + auto result = + std::make_shared(); oss << "Failed to create task: " << e.what(); result->message = oss.str(); result->success = false; @@ -392,8 +396,9 @@ auto WorkcellOrchestrator::_configure( }); this->_wc_state_pub = - this->create_publisher(endpoints::WorkcellStateTopic::topic_name( - this->get_name()), endpoints::WorkcellStateTopic::qos()); + this->create_publisher( + endpoints::WorkcellStateTopic::topic_name(this->get_name()), + endpoints::WorkcellStateTopic::qos()); this->_cur_state = WorkcellState(); this->_cur_state.workcell_id = this->get_name(); this->_cur_state.status = WorkcellState::STATUS_IDLE; @@ -411,88 +416,86 @@ auto WorkcellOrchestrator::_configure( }); this->_signal_wc_srv = - this->create_service(endpoints::SignalWorkcellService::service_name( - this->get_name()), - [this](endpoints::SignalWorkcellService::ServiceType::Request:: - ConstSharedPtr req, - endpoints::SignalWorkcellService::ServiceType::Response::SharedPtr resp) - { - this->_process_signal(req, resp); - }); + this->create_service( + endpoints::SignalWorkcellService::service_name(this->get_name()), + [this](endpoints::SignalWorkcellService::ServiceType::Request:: + ConstSharedPtr req, + endpoints::SignalWorkcellService::ServiceType::Response::SharedPtr resp) + { + this->_process_signal(req, resp); + }); // create pause service this->_pause_srv = - this->create_service(endpoints::PauseWorkcellService::service_name( - this->get_name()), - [this](endpoints::PauseWorkcellService::ServiceType::Request:: - ConstSharedPtr req, - endpoints::PauseWorkcellService::ServiceType::Response::SharedPtr resp) - { - this->_paused = req->pause; - resp->success = true; - std::string verb = req->pause ? "paused" : "unpaused"; - RCLCPP_INFO(this->get_logger(), "workcell %s", verb.c_str()); - }); + this->create_service( + endpoints::PauseWorkcellService::service_name(this->get_name()), + [this](endpoints::PauseWorkcellService::ServiceType::Request:: + ConstSharedPtr req, + endpoints::PauseWorkcellService::ServiceType::Response::SharedPtr resp) + { + this->_paused = req->pause; + resp->success = true; + std::string verb = req->pause ? "paused" : "unpaused"; + RCLCPP_INFO(this->get_logger(), "workcell %s", verb.c_str()); + }); this->_queue_task_srv = - this->create_service(endpoints::QueueWorkcellTaskService::service_name( - this->get_name()), - [this](endpoints::QueueWorkcellTaskService::ServiceType::Request:: - ConstSharedPtr req, - endpoints::QueueWorkcellTaskService::ServiceType::Response::SharedPtr resp) + this->create_service( + endpoints::QueueWorkcellTaskService::service_name(this->get_name()), + [this](endpoints::QueueWorkcellTaskService::ServiceType::Request:: + ConstSharedPtr req, + endpoints::QueueWorkcellTaskService::ServiceType::Response::SharedPtr resp) + { + const auto it = std::find_if(this->_ctxs.begin(), this->_ctxs.end(), + [&req](const std::shared_ptr& ctx) { - const auto it = - std::find_if(this->_ctxs.begin(), this->_ctxs.end(), - [&req](const std::shared_ptr& ctx) - { - return ctx->task.id == req->task_id; - }); - if (it != this->_ctxs.end()) - { - resp->success = false; - resp->message = "A task with the same id already exists"; - RCLCPP_INFO(this->get_logger(), "Failed to queue task [%s]: %s", - req->task_id.c_str(), resp->message.c_str()); - return; - } - const auto& ctx = - this->_ctxs.emplace_back(std::make_shared(*this)); - ctx->task.id = req->task_id; - ctx->task_state.task_id = req->task_id; - ctx->task_state.workcell_id = this->get_name(); - ctx->task_state.status = TaskState::STATUS_ASSIGNED; - resp->success = true; - RCLCPP_INFO(this->get_logger(), "queued task %s", ctx->task.id.c_str()); + return ctx->task.id == req->task_id; }); + if (it != this->_ctxs.end()) + { + resp->success = false; + resp->message = "A task with the same id already exists"; + RCLCPP_INFO(this->get_logger(), "Failed to queue task [%s]: %s", + req->task_id.c_str(), resp->message.c_str()); + return; + } + const auto& ctx = + this->_ctxs.emplace_back(std::make_shared(*this)); + ctx->task.id = req->task_id; + ctx->task_state.task_id = req->task_id; + ctx->task_state.workcell_id = this->get_name(); + ctx->task_state.status = TaskState::STATUS_ASSIGNED; + resp->success = true; + RCLCPP_INFO(this->get_logger(), "queued task %s", ctx->task.id.c_str()); + }); this->_remove_pending_task_srv = - this->create_service(endpoints::RemovePendingTaskService::service_name( - this->get_name()), - [this](endpoints::RemovePendingTaskService::ServiceType::Request:: - ConstSharedPtr req, - endpoints::RemovePendingTaskService::ServiceType::Response::SharedPtr resp) + this->create_service( + endpoints::RemovePendingTaskService::service_name(this->get_name()), + [this](endpoints::RemovePendingTaskService::ServiceType::Request:: + ConstSharedPtr req, + endpoints::RemovePendingTaskService::ServiceType::Response::SharedPtr resp) + { + RCLCPP_DEBUG(this->get_logger(), + "received request to remove pending task [%s]", req->task_id.c_str()); + const auto it = std::find_if(this->_ctxs.begin(), this->_ctxs.end(), + [&req](const std::shared_ptr& ctx) { - RCLCPP_DEBUG(this->get_logger(), - "received request to remove pending task [%s]", req->task_id.c_str()); - const auto it = - std::find_if(this->_ctxs.begin(), this->_ctxs.end(), - [&req](const std::shared_ptr& ctx) - { - return ctx->task.id == req->task_id; - }); - if (it == this->_ctxs.end() || - (*it)->task_state.status != TaskState::STATUS_ASSIGNED) - { - resp->success = false; - resp->message = "task does not exist or is not pending"; - RCLCPP_DEBUG_STREAM(this->get_logger(), resp->message); - return; - } - this->_ctxs.erase(it); - RCLCPP_INFO(this->get_logger(), "removed task [%s]", - req->task_id.c_str()); - resp->success = true; + return ctx->task.id == req->task_id; }); + if (it == this->_ctxs.end() || + (*it)->task_state.status != TaskState::STATUS_ASSIGNED) + { + resp->success = false; + resp->message = "task does not exist or is not pending"; + RCLCPP_DEBUG_STREAM(this->get_logger(), resp->message); + return; + } + this->_ctxs.erase(it); + RCLCPP_INFO(this->get_logger(), "removed task [%s]", + req->task_id.c_str()); + resp->success = true; + }); this->_tf2_buffer = std::make_shared(this->get_clock()); this->_tf2_listener = std::make_unique( @@ -581,7 +584,7 @@ auto WorkcellOrchestrator::_configure( [this](const std::string& name, const BT::NodeConfiguration& config) { return std::make_unique(name, config, *this, - this->_tf2_buffer); + this->_tf2_buffer); }); this->_bt_factory->registerBuilder( @@ -704,9 +707,11 @@ void WorkcellOrchestrator::_tick_bt(const std::shared_ptr& ctx) if (ctx->tick_count == 1) { RCLCPP_INFO( - this->get_logger(), "Task [%s] is pending, call \"%s\" to start this task", + this->get_logger(), + "Task [%s] is pending, call \"%s\" to start this task", ctx->task.id.c_str(), - endpoints::WorkcellRequestAction::action_name(this->get_name()).c_str()); + endpoints::WorkcellRequestAction::action_name( + this->get_name()).c_str()); } return; } @@ -774,7 +779,8 @@ void WorkcellOrchestrator::_tick_all_bts() ctx->goal_handle->is_canceling()) { RCLCPP_WARN( - this->get_logger(), "Cancelling all tasks because an ongoing task [%s] is being cancelled", + this->get_logger(), + "Cancelling all tasks because an ongoing task [%s] is being cancelled", ctx->task.id.c_str()); // NOTE: iterators are invalidated, it is important to // not access them after this line. @@ -869,7 +875,7 @@ void WorkcellOrchestrator::_register() RCLCPP_INFO(this->get_logger(), "Registering with system orchestrator"); auto register_cb = [this](rclcpp::Client:: - SharedFuture future) + SharedFuture future) { this->_ongoing_register = std::nullopt; auto resp = future.get(); diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp index 8e2b539..4abe5b2 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp @@ -147,7 +147,8 @@ private: void _cancel_all_tasks(); private: void _register(); private: void _handle_register_response( - endpoints::RegisterWorkcellService::ServiceType::Response::ConstSharedPtr resp); + endpoints::RegisterWorkcellService::ServiceType::Response::ConstSharedPtr + resp); private: void _process_signal( endpoints::SignalWorkcellService::ServiceType::Request::ConstSharedPtr req,