From 9026d79c3e25974deb4757a11e2308e0f50e03ed Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 27 Jan 2023 17:40:26 +0100 Subject: [PATCH] lint, uncrustify --- .../src/robot_manager_node.cpp | 50 +++++++++---------- .../kuka_fri_hardware_interface.hpp | 2 +- .../kuka_fri_hardware_interface.cpp | 4 +- .../src/kuka_sunrise/robot_manager_node.cpp | 50 ++++++++++--------- 4 files changed, 53 insertions(+), 53 deletions(-) diff --git a/kuka_rox_hardware_interface/src/robot_manager_node.cpp b/kuka_rox_hardware_interface/src/robot_manager_node.cpp index 92bfcb2e..12049f2c 100644 --- a/kuka_rox_hardware_interface/src/robot_manager_node.cpp +++ b/kuka_rox_hardware_interface/src/robot_manager_node.cpp @@ -14,6 +14,8 @@ #include "kuka_rox_hw_interface/robot_manager_node.hpp" +using namespace controller_manager_msgs::srv; // NOLINT + namespace kuka_rox { @@ -25,11 +27,11 @@ RobotManagerNode::RobotManagerNode() qos.reliable(); cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); change_hardware_state_client_ = - this->create_client( + this->create_client( "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_ ); change_controller_state_client_ = - this->create_client( + this->create_client( "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_ ); @@ -46,13 +48,12 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { // Configure hardware interface auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iisy_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kroshu_ros2_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000 - ); + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); return FAILURE; @@ -69,13 +70,12 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { // Cleanup hardware interface auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iisy_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kroshu_ros2_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000 - ); + kroshu_ros2_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"); return FAILURE; @@ -116,13 +116,12 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { // Activate hardware interface auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iisy_hardware"; hw_request->target_state.label = "active"; auto hw_response = - kroshu_ros2_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000 - ); + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); // 'unset config' does not exist, safe to return @@ -133,11 +132,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate joint state broadcaster auto controller_request = - std::make_shared(); - controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::make_shared(); + controller_request->strictness = SwitchController::Request::STRICT; controller_request->activate_controllers = std::vector{"joint_state_broadcaster"}; auto controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000 ); if (!controller_response || !controller_response->ok) { @@ -159,10 +158,10 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) } // Activate RT commander - controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_request->strictness = SwitchController::Request::STRICT; controller_request->activate_controllers = controller_names_; controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000 ); if (!controller_response || !controller_response->ok) { @@ -181,13 +180,12 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) { // Deactivate hardware interface auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iisy_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kroshu_ros2_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000 - ); + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; @@ -198,13 +196,13 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers controller_names_.emplace_back("joint_state_broadcaster"); auto controller_request = - std::make_shared(); + std::make_shared(); // With best effort strictness, deactivation succeeds if specific controller is not active controller_request->strictness = - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + SwitchController::Request::BEST_EFFORT; controller_request->deactivate_controllers = controller_names_; auto controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000 ); if (!controller_response || !controller_response->ok) { diff --git a/kuka_sunrise_driver/include/kuka_sunrise/kuka_fri_hardware_interface.hpp b/kuka_sunrise_driver/include/kuka_sunrise/kuka_fri_hardware_interface.hpp index 698a79fd..f2ef5af0 100644 --- a/kuka_sunrise_driver/include/kuka_sunrise/kuka_fri_hardware_interface.hpp +++ b/kuka_sunrise_driver/include/kuka_sunrise/kuka_fri_hardware_interface.hpp @@ -53,7 +53,7 @@ static std::unordered_mapcreate_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); change_hardware_state_client_ = - this->create_client( + this->create_client( "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_); change_controller_state_client_ = - this->create_client( + this->create_client( "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_); command_state_changed_publisher_ = this->create_publisher( "robot_manager/commanding_state_changed", qos); @@ -64,11 +66,11 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { // Configure hardware interface auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); @@ -88,12 +90,12 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Start non-RT controllers auto controller_request = - std::make_shared(); - controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::make_shared(); + controller_request->strictness = SwitchController::Request::STRICT; controller_request->activate_controllers = std::vector{"timing_controller", "robot_state_broadcaster"}; auto controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 3000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not start controllers"); @@ -144,14 +146,14 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) // Stop non-RT controllers auto controller_request = - std::make_shared(); + std::make_shared(); // With best effort strictness, cleanup succeeds if specific controller is not active controller_request->strictness = - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + SwitchController::Request::BEST_EFFORT; controller_request->deactivate_controllers = std::vector{"timing_controller", "robot_state_broadcaster"}; auto controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); @@ -161,11 +163,11 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) // Cleanup hardware interface // If it is inactive, cleanup will also succeed auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "unconfigured"; auto hw_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_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"); @@ -229,11 +231,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate hardware interface auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "active"; auto hw_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); @@ -254,11 +256,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // 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 = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::make_shared(); + controller_request->strictness = SwitchController::Request::STRICT; controller_request->activate_controllers = std::vector{"joint_state_broadcaster"}; auto controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_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"); @@ -271,10 +273,10 @@ 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 = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_request->strictness = SwitchController::Request::STRICT; controller_request->activate_controllers = std::vector{controller_name_}; controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not activate controller"); @@ -313,11 +315,11 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Deactivate hardware interface // If it is inactive, deactivation will also succeed auto hw_request = - std::make_shared(); + std::make_shared(); hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); @@ -327,14 +329,14 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers auto controller_request = - std::make_shared(); + std::make_shared(); // With best effort strictness, deactivation succeeds if specific controller is not active controller_request->strictness = - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + SwitchController::Request::BEST_EFFORT; controller_request->deactivate_controllers = std::vector{controller_name_, "joint_state_broadcaster"}; auto controller_response = - kroshu_ros2_core::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not stop controllers");