From c8c35973941c3d494fda254f64f113082c2ae5d6 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Thu, 14 Dec 2023 11:47:10 +0100 Subject: [PATCH] remove sendrequests --- .../src/robot_manager_node.cpp | 4 - .../src/robot_manager_node.cpp | 84 ++++-------- .../src/robot_manager_node.cpp | 124 ++++++------------ 3 files changed, 71 insertions(+), 141 deletions(-) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 82698812..de9caccf 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -376,8 +376,6 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) if (is_active_state) { // The driver is in active state // Activate controllers needed for the new control mode - auto controller_request = - std::make_shared(); if (!switch_controllers.first.empty()) { if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, switch_controllers.first, {})) @@ -421,8 +419,6 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) RCLCPP_INFO(get_logger(), "Robot Controller finished control mode change"); #endif - auto controller_request = - std::make_shared(); // Deactivate unnecessary controllers if (!switch_controllers.second.empty()) { if (!kuka_drivers_core::changeControllerState( diff --git a/kuka_kss_rsi_driver/src/robot_manager_node.cpp b/kuka_kss_rsi_driver/src/robot_manager_node.cpp index bc5faabd..bfe4965d 100644 --- a/kuka_kss_rsi_driver/src/robot_manager_node.cpp +++ b/kuka_kss_rsi_driver/src/robot_manager_node.cpp @@ -15,6 +15,8 @@ #include "kuka_kss_rsi_driver/robot_manager_node.hpp" #include "kuka_drivers_core/control_mode.hpp" +#include "communication_helpers/ros2_control_tools.hpp" + using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT @@ -55,14 +57,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { // Configure hardware interface - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000); - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_INACTIVE)) + { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); return FAILURE; } @@ -78,14 +76,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { // Clean up hardware interface - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.id = State::PRIMARY_STATE_UNCONFIGURED; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000); - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_UNCONFIGURED)) + { RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); return FAILURE; } @@ -104,32 +98,21 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { // Activate hardware interface - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.id = State::PRIMARY_STATE_ACTIVE; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 10000); - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_ACTIVE, 10000)) + { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); return FAILURE; } // Activate RT controller(s) - auto controller_request = - std::make_shared(); - controller_request->strictness = SwitchController::Request::STRICT; - controller_request->activate_controllers = - {"joint_state_broadcaster", "joint_trajectory_controller"}; - - auto controller_response = - kuka_drivers_core::sendRequest( - change_controller_state_client_, controller_request, 0, 2000 - ); - if (!controller_response || !controller_response->ok) { - RCLCPP_ERROR(get_logger(), "Could not activate controller"); + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {"joint_state_broadcaster", "joint_trajectory_controller"}, + {})) + { + RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); // TODO(Svastits): this can be removed if rollback is implemented properly this->on_deactivate(get_current_state()); return FAILURE; @@ -143,34 +126,23 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) { // Deactivate hardware interface - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 3000); // was not stable with 2000 ms timeout - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_INACTIVE)) + { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; } RCLCPP_INFO(get_logger(), "Deactivated hardware interface"); - // Stop RT controllers - auto controller_request = - std::make_shared(); // With best effort strictness, deactivation succeeds if specific controller is not active - controller_request->strictness = - SwitchController::Request::BEST_EFFORT; - controller_request->deactivate_controllers = - {"joint_state_broadcaster", "joint_trajectory_controller"}; - auto controller_response = - kuka_drivers_core::sendRequest( - change_controller_state_client_, controller_request, 0, 2000 - ); - if (!controller_response || !controller_response->ok) { + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {}, + {"joint_state_broadcaster", "joint_trajectory_controller"}, + SwitchController::Request::BEST_EFFORT)) + { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); return ERROR; } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index f5ff5b34..044f0cb9 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -71,14 +71,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { // Configure hardware interface - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.label = "inactive"; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000); - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_INACTIVE)) + { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); return FAILURE; } @@ -95,16 +91,11 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Successfully set 'controller_ip' parameter"); // Start non-RT controllers - auto controller_request = - std::make_shared(); - controller_request->strictness = SwitchController::Request::STRICT; - controller_request->activate_controllers = - std::vector{"timing_controller", "fri_state_broadcaster"}; - auto controller_response = - kuka_drivers_core::sendRequest( - change_controller_state_client_, controller_request, 0, 3000); - if (!controller_response || !controller_response->ok) { - RCLCPP_ERROR(get_logger(), "Could not start controllers"); + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {"fri_configuration_controller", "fri_state_broadcaster"}, + {})) + { + RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); result = FAILURE; } @@ -151,32 +142,23 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) } // Stop non-RT controllers - auto controller_request = - std::make_shared(); // With best effort strictness, cleanup succeeds if specific controller is not active - controller_request->strictness = - SwitchController::Request::BEST_EFFORT; - controller_request->deactivate_controllers = - std::vector{"fri_configuration_controller", "fri_state_broadcaster"}; - auto controller_response = - kuka_drivers_core::sendRequest( - change_controller_state_client_, controller_request, 0, 2000); - if (!controller_response || !controller_response->ok) { + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {}, + {"fri_configuration_controller", "fri_state_broadcaster"}, + SwitchController::Request::BEST_EFFORT)) + { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); return ERROR; } // Cleanup hardware interface // If it is inactive, cleanup will also succeed - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.label = "unconfigured"; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000); - if (!hw_response || !hw_response->ok) { - RCLCPP_ERROR(get_logger(), "Could not cleanup hardware interface"); + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_UNCONFIGURED)) + { + RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); return FAILURE; } @@ -209,18 +191,15 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Successfully set FRI config"); // Activate hardware interface - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.label = "active"; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000); - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_ACTIVE)) + { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); // 'unset config' does not exist, safe to return return FAILURE; } + RCLCPP_INFO(get_logger(), "Activated LBR iiwa hardware interface"); // Start FRI (in monitoring mode) @@ -233,16 +212,12 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate joint state broadcaster // The other controller must be started later so that it can initialize internal state - // with broadcaster information - auto controller_request = - std::make_shared(); - controller_request->strictness = SwitchController::Request::STRICT; - controller_request->activate_controllers = std::vector{"joint_state_broadcaster"}; - auto controller_response = - kuka_drivers_core::sendRequest( - change_controller_state_client_, controller_request, 0, 2000); - if (!controller_response || !controller_response->ok) { - RCLCPP_ERROR(get_logger(), "Could not start joint state broadcaster"); + // with broadcaster information -> TODO(Svastits): validate whether this is true + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {"joint_state_broadcaster"}, + {})) + { + RCLCPP_ERROR(get_logger(), "Could not activate joint state broadcaster"); this->on_deactivate(get_current_state()); return FAILURE; } @@ -252,13 +227,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) controller_name_ = (this->get_parameter("command_mode").as_string() == "position") ? position_controller_name : torque_controller_name; // Activate RT commander - controller_request->strictness = SwitchController::Request::STRICT; - controller_request->activate_controllers = std::vector{controller_name_}; - controller_response = - kuka_drivers_core::sendRequest( - change_controller_state_client_, controller_request, 0, 2000); - if (!controller_response || !controller_response->ok) { - RCLCPP_ERROR(get_logger(), "Could not activate controller"); + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {controller_name_}, + {})) + { + RCLCPP_ERROR(get_logger(), "Could not activate RT controller"); this->on_deactivate(get_current_state()); return FAILURE; } @@ -293,32 +266,21 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Deactivate hardware interface // If it is inactive, deactivation will also succeed - auto hw_request = - std::make_shared(); - hw_request->name = robot_model_; - hw_request->target_state.label = "inactive"; - auto hw_response = - kuka_drivers_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000); - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_INACTIVE)) + { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; } + RCLCPP_INFO(get_logger(), "Deactivated LBR iiwa hardware interface"); // Stop RT controllers - auto controller_request = - std::make_shared(); // With best effort strictness, deactivation succeeds if specific controller is not active - controller_request->strictness = - SwitchController::Request::BEST_EFFORT; - controller_request->deactivate_controllers = - std::vector{controller_name_, "joint_state_broadcaster"}; - auto controller_response = - kuka_drivers_core::sendRequest( - change_controller_state_client_, controller_request, 0, 2000); - if (!controller_response || !controller_response->ok) { - RCLCPP_ERROR(get_logger(), "Could not stop controllers"); + if (!kuka_drivers_core::changeControllerState(change_controller_state_client_, {}, + {controller_name_, "joint_state_broadcaster"}, SwitchController::Request::BEST_EFFORT)) { + RCLCPP_ERROR(get_logger(), "Could not deactivate controllers"); return ERROR; }