From dc4674a8d094643719e8c8035feee745b72f147c Mon Sep 17 00:00:00 2001 From: Svastits <49677296+Svastits@users.noreply.github.com> Date: Thu, 14 Dec 2023 15:55:33 +0100 Subject: [PATCH] Remove robot manager code duplications (#122) * add ros2 control tools to core * snake_case file names * uset tools * finish iiqka restruct * remove sendrequests * remove some logs * fix build * clean up includes + lint * fix bug * combine conditions * lint --------- Co-authored-by: Aron Svastits --- kuka_drivers_core/CMakeLists.txt | 8 +- .../ros2_control_tools.hpp | 67 +++++++ .../{ControlMode.hpp => control_mode.hpp} | 6 +- ...llerHandler.hpp => controller_handler.hpp} | 8 +- ...meterHandler.hpp => parameter_handler.hpp} | 6 +- ...S2BaseLCNode.hpp => ros2_base_lc_node.hpp} | 8 +- .../{ROS2BaseNode.hpp => ros2_base_node.hpp} | 8 +- ...llerHandler.cpp => controller_handler.cpp} | 2 +- ...meterHandler.cpp => parameter_handler.cpp} | 2 +- ...S2BaseLCNode.cpp => ros2_base_lc_node.cpp} | 2 +- .../{ROS2BaseNode.cpp => ros2_base_node.cpp} | 2 +- .../hardware_interface.hpp | 10 +- .../robot_manager_node.hpp | 12 +- .../src/hardware_interface.cpp | 7 +- .../src/robot_manager_node.cpp | 186 ++++++------------ .../hardware_interface.hpp | 20 +- .../robot_manager_node.hpp | 7 +- .../src/hardware_interface.cpp | 2 + .../src/robot_manager_node.cpp | 91 +++------ .../configuration_manager.hpp | 2 +- .../hardware_interface.hpp | 10 +- .../robot_manager_node.hpp | 10 +- .../src/hardware_interface.cpp | 2 + .../src/robot_manager_node.cpp | 130 +++++------- 24 files changed, 260 insertions(+), 348 deletions(-) create mode 100644 kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp rename kuka_drivers_core/include/kuka_drivers_core/{ControlMode.hpp => control_mode.hpp} (91%) rename kuka_drivers_core/include/kuka_drivers_core/{ControllerHandler.hpp => controller_handler.hpp} (95%) rename kuka_drivers_core/include/kuka_drivers_core/{ParameterHandler.hpp => parameter_handler.hpp} (97%) rename kuka_drivers_core/include/kuka_drivers_core/{ROS2BaseLCNode.hpp => ros2_base_lc_node.hpp} (94%) rename kuka_drivers_core/include/kuka_drivers_core/{ROS2BaseNode.hpp => ros2_base_node.hpp} (90%) rename kuka_drivers_core/src/{ControllerHandler.cpp => controller_handler.cpp} (99%) rename kuka_drivers_core/src/{ParameterHandler.cpp => parameter_handler.cpp} (98%) rename kuka_drivers_core/src/{ROS2BaseLCNode.cpp => ros2_base_lc_node.cpp} (98%) rename kuka_drivers_core/src/{ROS2BaseNode.cpp => ros2_base_node.cpp} (96%) diff --git a/kuka_drivers_core/CMakeLists.txt b/kuka_drivers_core/CMakeLists.txt index 1e4c09bc..2a73eb83 100644 --- a/kuka_drivers_core/CMakeLists.txt +++ b/kuka_drivers_core/CMakeLists.txt @@ -22,10 +22,10 @@ find_package(lifecycle_msgs REQUIRED) find_package(controller_manager REQUIRED) add_library(kuka_drivers_core SHARED - src/ROS2BaseNode.cpp - src/ROS2BaseLCNode.cpp - src/ParameterHandler.cpp - src/ControllerHandler.cpp + src/ros2_base_node.cpp + src/ros2_base_lc_node.cpp + src/parameter_handler.cpp + src/controller_handler.cpp ) ament_target_dependencies(kuka_drivers_core rclcpp rclcpp_lifecycle lifecycle_msgs) diff --git a/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp b/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp new file mode 100644 index 00000000..259d17cc --- /dev/null +++ b/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp @@ -0,0 +1,67 @@ +// Copyright 2020 Áron Svastits +// +// 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 COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_ +#define COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "communication_helpers/service_tools.hpp" + +namespace kuka_drivers_core +{ + +bool changeHardwareState( + rclcpp::Client::SharedPtr client, + const std::string & hardware_name, uint8_t state, int timeout_ms = 2000) +{ + auto hw_request = + std::make_shared(); + hw_request->name = hardware_name; + hw_request->target_state.id = state; + auto hw_response = sendRequest( + client, hw_request, 0, timeout_ms); + if (!hw_response || !hw_response->ok) { + return false; + } + return true; +} + +bool changeControllerState( + rclcpp::Client::SharedPtr client, + const std::vector & activate_controllers, + const std::vector & deactivate_controllers, + int32_t strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT) +{ + auto controller_request = + std::make_shared(); + controller_request->strictness = strictness; + controller_request->activate_controllers = activate_controllers; + controller_request->deactivate_controllers = deactivate_controllers; + + auto controller_response = sendRequest( + client, controller_request, 0, 2000); + if (!controller_response || !controller_response->ok) { + return false; + } + return true; +} +} // namespace kuka_drivers_core + +#endif // COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/ControlMode.hpp b/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp similarity index 91% rename from kuka_drivers_core/include/kuka_drivers_core/ControlMode.hpp rename to kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp index 8955f29b..4a743ba4 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/ControlMode.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp @@ -13,8 +13,8 @@ // limitations under the License. -#ifndef KUKA_DRIVERS_CORE__CONTROLMODE_HPP_ -#define KUKA_DRIVERS_CORE__CONTROLMODE_HPP_ +#ifndef KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_ +#define KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_ namespace kuka_drivers_core { @@ -51,4 +51,4 @@ enum class ControllerType : std::uint8_t }; } // namespace kuka_drivers_core -#endif // KUKA_DRIVERS_CORE__CONTROLMODE_HPP_ +#endif // KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/ControllerHandler.hpp b/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp similarity index 95% rename from kuka_drivers_core/include/kuka_drivers_core/ControllerHandler.hpp rename to kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp index 8037f9e1..4eeb7850 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/ControllerHandler.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KUKA_DRIVERS_CORE__CONTROLLERHANDLER_HPP_ -#define KUKA_DRIVERS_CORE__CONTROLLERHANDLER_HPP_ +#ifndef KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_ +#define KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_ #include #include @@ -22,7 +22,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "ControlMode.hpp" +#include "control_mode.hpp" namespace kuka_drivers_core { @@ -128,4 +128,4 @@ class ControllerHandler } // namespace kuka_drivers_core -#endif // KUKA_DRIVERS_CORE__CONTROLLERHANDLER_HPP_ +#endif // KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/ParameterHandler.hpp b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp similarity index 97% rename from kuka_drivers_core/include/kuka_drivers_core/ParameterHandler.hpp rename to kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp index 67eb3da1..ec1f5b13 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/ParameterHandler.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KUKA_DRIVERS_CORE__PARAMETERHANDLER_HPP_ -#define KUKA_DRIVERS_CORE__PARAMETERHANDLER_HPP_ +#ifndef KUKA_DRIVERS_CORE__PARAMETER_HANDLER_HPP_ +#define KUKA_DRIVERS_CORE__PARAMETER_HANDLER_HPP_ #include #include @@ -180,4 +180,4 @@ class ParameterHandler } // namespace kuka_drivers_core -#endif // KUKA_DRIVERS_CORE__PARAMETERHANDLER_HPP_ +#endif // KUKA_DRIVERS_CORE__PARAMETER_HANDLER_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/ROS2BaseLCNode.hpp b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_lc_node.hpp similarity index 94% rename from kuka_drivers_core/include/kuka_drivers_core/ROS2BaseLCNode.hpp rename to kuka_drivers_core/include/kuka_drivers_core/ros2_base_lc_node.hpp index dc491ba2..f2e573b2 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/ROS2BaseLCNode.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_lc_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KUKA_DRIVERS_CORE__ROS2BASELCNODE_HPP_ -#define KUKA_DRIVERS_CORE__ROS2BASELCNODE_HPP_ +#ifndef KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_ +#define KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_ #include #include @@ -23,7 +23,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "kuka_drivers_core/ParameterHandler.hpp" +#include "kuka_drivers_core/parameter_handler.hpp" namespace kuka_drivers_core { @@ -90,4 +90,4 @@ class ROS2BaseLCNode : public rclcpp_lifecycle::LifecycleNode } // namespace kuka_drivers_core -#endif // KUKA_DRIVERS_CORE__ROS2BASELCNODE_HPP_ +#endif // KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/ROS2BaseNode.hpp b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_node.hpp similarity index 90% rename from kuka_drivers_core/include/kuka_drivers_core/ROS2BaseNode.hpp rename to kuka_drivers_core/include/kuka_drivers_core/ros2_base_node.hpp index 180e1720..ebce99e0 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/ROS2BaseNode.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KUKA_DRIVERS_CORE__ROS2BASENODE_HPP_ -#define KUKA_DRIVERS_CORE__ROS2BASENODE_HPP_ +#ifndef KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_ +#define KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_ #include #include @@ -24,7 +24,7 @@ #include "rclcpp/node.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "kuka_drivers_core/ParameterHandler.hpp" +#include "kuka_drivers_core/parameter_handler.hpp" namespace kuka_drivers_core { @@ -65,4 +65,4 @@ class ROS2BaseNode : public rclcpp::Node }; } // namespace kuka_drivers_core -#endif // KUKA_DRIVERS_CORE__ROS2BASENODE_HPP_ +#endif // KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_ diff --git a/kuka_drivers_core/src/ControllerHandler.cpp b/kuka_drivers_core/src/controller_handler.cpp similarity index 99% rename from kuka_drivers_core/src/ControllerHandler.cpp rename to kuka_drivers_core/src/controller_handler.cpp index 8ac0f59c..0438c837 100644 --- a/kuka_drivers_core/src/ControllerHandler.cpp +++ b/kuka_drivers_core/src/controller_handler.cpp @@ -15,7 +15,7 @@ #include #include -#include "kuka_drivers_core/ControllerHandler.hpp" +#include "kuka_drivers_core/controller_handler.hpp" namespace kuka_drivers_core { diff --git a/kuka_drivers_core/src/ParameterHandler.cpp b/kuka_drivers_core/src/parameter_handler.cpp similarity index 98% rename from kuka_drivers_core/src/ParameterHandler.cpp rename to kuka_drivers_core/src/parameter_handler.cpp index 2403c80d..83cfb58b 100644 --- a/kuka_drivers_core/src/ParameterHandler.cpp +++ b/kuka_drivers_core/src/parameter_handler.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kuka_drivers_core/ParameterHandler.hpp" +#include "kuka_drivers_core/parameter_handler.hpp" #include #include diff --git a/kuka_drivers_core/src/ROS2BaseLCNode.cpp b/kuka_drivers_core/src/ros2_base_lc_node.cpp similarity index 98% rename from kuka_drivers_core/src/ROS2BaseLCNode.cpp rename to kuka_drivers_core/src/ros2_base_lc_node.cpp index 781bb5f4..92728252 100644 --- a/kuka_drivers_core/src/ROS2BaseLCNode.cpp +++ b/kuka_drivers_core/src/ros2_base_lc_node.cpp @@ -16,7 +16,7 @@ #include #include -#include "kuka_drivers_core/ROS2BaseLCNode.hpp" +#include "kuka_drivers_core/ros2_base_lc_node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace kuka_drivers_core diff --git a/kuka_drivers_core/src/ROS2BaseNode.cpp b/kuka_drivers_core/src/ros2_base_node.cpp similarity index 96% rename from kuka_drivers_core/src/ROS2BaseNode.cpp rename to kuka_drivers_core/src/ros2_base_node.cpp index 47bbe5db..e0788a3a 100644 --- a/kuka_drivers_core/src/ROS2BaseNode.cpp +++ b/kuka_drivers_core/src/ros2_base_node.cpp @@ -16,7 +16,7 @@ #include #include -#include "kuka_drivers_core/ROS2BaseNode.hpp" +#include "kuka_drivers_core/ros2_base_node.hpp" namespace kuka_drivers_core diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index 6c863cd5..ca81911b 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -23,17 +23,11 @@ #include #include -#include "rclcpp/macros.hpp" - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" - #include "pluginlib/class_list_macros.hpp" +#include "hardware_interface/system_interface.hpp" #include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h" #include "nanopb/kuka/core/motion/joint.pb.hh" diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index 75c5b2d2..f95558f2 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -22,16 +22,13 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/client.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" -#include "controller_manager_msgs/srv/switch_controller.hpp" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/u_int32.hpp" +#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" -#include "communication_helpers/service_tools.hpp" -#include "kuka_drivers_core/ROS2BaseLCNode.hpp" -#include "kuka_drivers_core/ControllerHandler.hpp" +#include "kuka_drivers_core/controller_handler.hpp" +#include "kuka_drivers_core/ros2_base_lc_node.hpp" #include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h" @@ -43,7 +40,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode RobotManagerNode(); ~RobotManagerNode(); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 023baad8..3d12e8f5 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kuka_iiqka_eac_driver/hardware_interface.hpp" - #include - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" + #include "nanopb-helpers/nanopb_serialization_helper.h" +#include "kuka_iiqka_eac_driver/hardware_interface.hpp" + using namespace kuka::ecs::v1; // NOLINT using os::core::udp::communication::Socket; diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 5683b304..604ad86f 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kuka_iiqka_eac_driver/robot_manager_node.hpp" +#include +#include "communication_helpers/ros2_control_tools.hpp" +#include "communication_helpers/service_tools.hpp" -#include +#include "kuka_iiqka_eac_driver/robot_manager_node.hpp" using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT @@ -131,38 +133,28 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) control_mode_pub_->publish(message); // 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; } - RCLCPP_INFO(get_logger(), "Successfully configured hardware interface"); is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); // Activate control mode handler - auto controller_request = - std::make_shared(); - controller_request->strictness = SwitchController::Request::STRICT; - controller_request->activate_controllers = {"control_mode_handler"}; - 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_, {"control_mode_handler"}, {})) + { + RCLCPP_ERROR(get_logger(), "Could not activate control mode handler"); // TODO(Svastits): this can be removed if rollback is implemented properly this->on_cleanup(get_current_state()); return FAILURE; } + RCLCPP_INFO(get_logger(), "Activated control mode handler"); return SUCCESS; @@ -172,27 +164,18 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { // Deactivate control mode handler - auto controller_request = - std::make_shared(); - controller_request->strictness = SwitchController::Request::STRICT; - controller_request->deactivate_controllers = {"control_mode_handler"}; - 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 deactivate controller"); + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {}, + {"control_mode_handler"})) + { + RCLCPP_ERROR(get_logger(), "Could not deactivate control mode handler"); } // 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; } @@ -262,14 +245,10 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) observe_thread_ = std::thread(&RobotManagerNode::ObserveControl, this); // 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, 5000); - if (!hw_response || !hw_response->ok) { + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_ACTIVE, 5000)) + { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); return FAILURE; } @@ -286,30 +265,24 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) return ERROR; } - // Activate RT controller(s) - auto controller_request = - std::make_shared(); - controller_request->strictness = SwitchController::Request::STRICT; - controller_request->activate_controllers = new_controllers.first; - + // Deactivate list for activation should always be empty, safety check if (!new_controllers.second.empty()) { - // This should never happen - controller_request->deactivate_controllers = new_controllers.second; RCLCPP_ERROR( get_logger(), "Controller handler state is improper, active controller list not empty before activation"); + return FAILURE; } - 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"); - // TODO(Svastits): this can be removed if rollback is implemented properly + // Activate RT controller(s) + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, new_controllers.first, + new_controllers.second)) + { + RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); this->on_deactivate(get_current_state()); return FAILURE; } + controller_handler_.ApproveControllerActivation(); if (!controller_handler_.ApproveControllerDeactivation()) { RCLCPP_ERROR( @@ -333,37 +306,24 @@ 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) { + // Deactivation was not stable with 2000 ms timeout + if (!kuka_drivers_core::changeHardwareState( + change_hardware_state_client_, robot_model_, + State::PRIMARY_STATE_INACTIVE, 3000)) + { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; } - RCLCPP_INFO(get_logger(), "Deactivated LBR iisy 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 = - controller_handler_.GetControllersForDeactivation(); - 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_handler_.GetControllersForDeactivation(), SwitchController::Request::BEST_EFFORT)) + { + RCLCPP_ERROR(get_logger(), "Could not stop RT controllers"); return ERROR; } + if (!controller_handler_.ApproveControllerDeactivation()) { RCLCPP_ERROR( get_logger(), @@ -395,13 +355,12 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) return false; } - std::pair, std::vector> switch_controllers; bool is_active_state = get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; - // Calculates the controller to activate and deactivate + // Determine which controllers to activate and deactivate try { switch_controllers = controller_handler_.GetControllersForSwitch( kuka_drivers_core::ControlMode(control_mode)); @@ -410,25 +369,15 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) return false; } - // Activate controllers + // Activate controllers needed for the new control mode if (is_active_state) { - // The driver is in active state - // Call request for activating controllers for the new control mode - auto controller_request = - std::make_shared(); - if (!switch_controllers.first.empty()) { - controller_request->activate_controllers = switch_controllers.first; - controller_request->strictness = SwitchController::Request::STRICT; - 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 controllers for new control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - return false; - } + if (!switch_controllers.first.empty() && !kuka_drivers_core::changeControllerState( + change_controller_state_client_, switch_controllers.first, {})) + { + RCLCPP_ERROR(get_logger(), "Could not activate controllers for new control mode"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return false; } controller_handler_.ApproveControllerActivation(); } @@ -463,23 +412,14 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) RCLCPP_INFO(get_logger(), "Robot Controller finished control mode change"); #endif - // Deactivate controllers - auto controller_request = - std::make_shared(); - // Call request for deactivating controllers for the new control mode - if (!switch_controllers.second.empty()) { - controller_request->deactivate_controllers = switch_controllers.second; - controller_request->strictness = SwitchController::Request::STRICT; - 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 deactivate controllers for new control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - return false; - } + // Deactivate unnecessary controllers + if (!switch_controllers.second.empty() && !kuka_drivers_core::changeControllerState( + change_controller_state_client_, {}, switch_controllers.second)) + { + RCLCPP_ERROR(get_logger(), "Could not deactivate controllers for new control mode"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return false; } if (!controller_handler_.ApproveControllerDeactivation()) { RCLCPP_ERROR( diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp index 6b060a20..5649afc1 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp @@ -41,10 +41,6 @@ #ifndef KUKA_KSS_RSI_DRIVER__HARDWARE_INTERFACE_HPP_ #define KUKA_KSS_RSI_DRIVER__HARDWARE_INTERFACE_HPP_ -#include -#include -#include - #include #include #include @@ -52,19 +48,17 @@ #include #include -#include "rclcpp/macros.hpp" - -#include "kuka_kss_rsi_driver/visibility_control.h" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/macros.hpp" +#include "pluginlib/class_list_macros.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "pluginlib/class_list_macros.hpp" +#include "hardware_interface/system_interface.hpp" +#include "kuka_kss_rsi_driver/udp_server.h" +#include "kuka_kss_rsi_driver/rsi_state.h" +#include "kuka_kss_rsi_driver/rsi_command.h" +#include "kuka_kss_rsi_driver/visibility_control.h" using hardware_interface::return_type; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp index 24ee2b01..ea96a9e0 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp @@ -22,14 +22,11 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/client.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/msg/state.hpp" +#include "std_msgs/msg/bool.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" -#include "std_msgs/msg/bool.hpp" -#include "communication_helpers/service_tools.hpp" -#include "kuka_drivers_core/ROS2BaseLCNode.hpp" +#include "kuka_drivers_core/ros2_base_lc_node.hpp" using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; diff --git a/kuka_kss_rsi_driver/src/hardware_interface.cpp b/kuka_kss_rsi_driver/src/hardware_interface.cpp index ffc36a2e..ab2a21c1 100644 --- a/kuka_kss_rsi_driver/src/hardware_interface.cpp +++ b/kuka_kss_rsi_driver/src/hardware_interface.cpp @@ -43,6 +43,8 @@ #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" + #include "kuka_kss_rsi_driver/hardware_interface.hpp" namespace kuka_kss_rsi_driver diff --git a/kuka_kss_rsi_driver/src/robot_manager_node.cpp b/kuka_kss_rsi_driver/src/robot_manager_node.cpp index a54c72de..3ab0fb9b 100644 --- a/kuka_kss_rsi_driver/src/robot_manager_node.cpp +++ b/kuka_kss_rsi_driver/src/robot_manager_node.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kuka_kss_rsi_driver/robot_manager_node.hpp" -#include "kuka_drivers_core/ControlMode.hpp" +#include "communication_helpers/ros2_control_tools.hpp" +#include "communication_helpers/service_tools.hpp" +#include "kuka_kss_rsi_driver/robot_manager_node.hpp" using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT @@ -55,18 +56,13 @@ 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; } - RCLCPP_INFO(get_logger(), "Successfully configured hardware interface"); is_configured_pub_->on_activate(); is_configured_msg_.data = true; @@ -78,14 +74,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 +96,20 @@ 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 +123,21 @@ 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/include/kuka_sunrise_fri_driver/configuration_manager.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp index 7062a5aa..cfb6f429 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp @@ -29,7 +29,7 @@ #include "controller_manager_msgs/srv/list_controllers.hpp" #include "kuka_sunrise_fri_driver/fri_connection.hpp" -#include "kuka_drivers_core/ROS2BaseLCNode.hpp" +#include "kuka_drivers_core/ros2_base_lc_node.hpp" namespace kuka_sunrise_fri_driver { diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index 8b34f545..e64d04da 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -22,21 +22,17 @@ #include #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include -#include +#include "pluginlib/class_list_macros.hpp" +#include "hardware_interface/system_interface.hpp" #include "kuka_driver_interfaces/srv/set_int.hpp" + #include "fri_client_sdk/friLBRClient.h" #include "fri_client_sdk/HWIFClientApplication.hpp" #include "fri_client_sdk/friUdpConnection.h" #include "fri_client_sdk/friClientIf.h" - #include "kuka_sunrise_fri_driver/visibility_control.h" - -#include "pluginlib/class_list_macros.hpp" - using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace kuka_sunrise_fri_driver diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 88902ea2..3361a5c8 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -21,19 +21,15 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/client.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" #include "std_msgs/msg/bool.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "kuka_drivers_core/ros2_base_lc_node.hpp" + #include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "kuka_sunrise_fri_driver/configuration_manager.hpp" -#include "communication_helpers/service_tools.hpp" - -#include "kuka_drivers_core/ROS2BaseLCNode.hpp" namespace kuka_sunrise_fri_driver { @@ -55,8 +51,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &); - // TODO(Svastits): implement on_error callback to clean up node - bool activateControl(); bool deactivateControl(); diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index fab1f84e..0d110ff7 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -14,6 +14,8 @@ #include +#include + #include "kuka_sunrise_fri_driver/hardware_interface.hpp" namespace kuka_sunrise_fri_driver diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index f5ff5b34..eed83785 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -14,13 +14,15 @@ #include +#include "communication_helpers/service_tools.hpp" +#include "communication_helpers/ros2_control_tools.hpp" + #include "kuka_sunrise_fri_driver/robot_manager_node.hpp" using namespace controller_manager_msgs::srv; // NOLINT namespace kuka_sunrise_fri_driver { - RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_manager") { @@ -71,14 +73,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_, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)) + { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); return FAILURE; } @@ -95,16 +93,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 +144,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_, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)) + { + RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); return FAILURE; } @@ -209,19 +193,14 @@ 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_, + lifecycle_msgs::msg::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) if (!fri_connection_->startFRI()) { @@ -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_, + lifecycle_msgs::msg::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; }