From 99524bd10110a27766e89a33fce69a01547a7626 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 5 Mar 2024 09:39:07 +0000 Subject: [PATCH] style(pre-commit): autofix --- .../CMakeLists.txt | 2 +- .../package.xml | 4 ++-- .../src/aw_pose_covariance_modifier_node.cpp | 24 +++++++++---------- .../aw_pose_covariance_modifier_node.hpp | 5 ++-- .../ndt_scan_matcher_core.hpp | 2 +- localization/ndt_scan_matcher/package.xml | 2 +- .../src/ndt_scan_matcher_core.cpp | 15 ++++++------ 7 files changed, 28 insertions(+), 26 deletions(-) diff --git a/localization/aw_pose_covariance_modifier_node/CMakeLists.txt b/localization/aw_pose_covariance_modifier_node/CMakeLists.txt index be045d1c14581..d14c20e381713 100644 --- a/localization/aw_pose_covariance_modifier_node/CMakeLists.txt +++ b/localization/aw_pose_covariance_modifier_node/CMakeLists.txt @@ -29,4 +29,4 @@ include_directories(src/include/) ament_auto_package(INSTALL_TO_SHARE - launch) \ No newline at end of file + launch) diff --git a/localization/aw_pose_covariance_modifier_node/package.xml b/localization/aw_pose_covariance_modifier_node/package.xml index 5df24a955c9be..4680527436c66 100644 --- a/localization/aw_pose_covariance_modifier_node/package.xml +++ b/localization/aw_pose_covariance_modifier_node/package.xml @@ -15,11 +15,11 @@ rosidl_default_generators geometry_msgs + rcl_interfaces rclcpp rclcpp_components - rcl_interfaces ament_cmake - \ No newline at end of file + diff --git a/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp b/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp index c68d607c16f3d..c89e00472409c 100644 --- a/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp +++ b/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp @@ -28,27 +28,23 @@ AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovar new_pose_estimator_pub_ = this->create_publisher( "output_pose_with_covariance_topic", 10); - client_ = - this->create_client("/localization/pose_estimator/ndt_scan_matcher/set_parameters"); + client_ = this->create_client( + "/localization/pose_estimator/ndt_scan_matcher/set_parameters"); while (!client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - this->get_logger(), - "Waiting for aw_pose_covariance_modifier_node service..."); + RCLCPP_INFO(this->get_logger(), "Waiting for aw_pose_covariance_modifier_node service..."); } activateNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier(); if (activateNDTCovModifier == 1) { RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ..."); - } - else{ + } else { RCLCPP_WARN(get_logger(), "Failed to enable NDT pose covariance modifier ..."); } } bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier() { - auto request = std::make_shared(); rcl_interfaces::msg::Parameter parameter; @@ -66,14 +62,18 @@ bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier() auto response = result_.get(); if (response && response->results.data()) { - RCLCPP_INFO(this->get_logger(), "aw_pose_covariance_modifier.enable parameter set successfully."); + RCLCPP_INFO( + this->get_logger(), "aw_pose_covariance_modifier.enable parameter set successfully."); return true; } else { - RCLCPP_ERROR(this->get_logger(), "An error occurred while setting the aw_pose_covariance_modifier.enable parameter."); + RCLCPP_ERROR( + this->get_logger(), + "An error occurred while setting the aw_pose_covariance_modifier.enable parameter."); return false; } } else { - RCLCPP_ERROR(this->get_logger(), "The request was not completed within the specified time."); + RCLCPP_ERROR( + this->get_logger(), "The request was not completed within the specified time."); return false; } }); @@ -109,4 +109,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp b/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp index 0da87150a2d0c..383f1665616a8 100644 --- a/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp +++ b/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp @@ -14,10 +14,11 @@ #ifndef AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_ #define AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_ +#include #include #include -#include + #include class AWPoseCovarianceModifierNode : public rclcpp::Node @@ -41,4 +42,4 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node bool activateNDTCovModifier = 0; }; -#endif // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_ \ No newline at end of file +#endif // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 0cf83cc935ee7..2deeaeaeae696 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -20,6 +20,7 @@ #include "localization_util/smart_pose_buffer.hpp" #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" #include #include @@ -42,7 +43,6 @@ #include #include #include -#include "rcl_interfaces/srv/set_parameters.hpp" #ifdef ROS_DISTRO_GALACTIC #include diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index a1d3fa823d93d..ba531dfa25a10 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -28,6 +28,7 @@ nav_msgs ndt_omp pcl_conversions + rcl_interfaces rclcpp sensor_msgs std_msgs @@ -42,7 +43,6 @@ tier4_localization_msgs tree_structured_parzen_estimator visualization_msgs - rcl_interfaces ament_cmake_cppcheck ament_lint_auto diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 679eff04d8a80..eae33b7d6ff00 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -198,12 +198,12 @@ void NDTScanMatcher::setParametersCallback( { std::lock_guard lock(mutex_cov_modifier_); - for (const auto ¶m : request->parameters) { - - if(param.name == "aw_pose_covariance_modifier.enable"){ - + for (const auto & param : request->parameters) { + if (param.name == "aw_pose_covariance_modifier.enable") { activate_pose_covariance_modifier_ = param.value.bool_value; - RCLCPP_INFO(this->get_logger(), "aw_pose_covariance_modifier.enable set to : %d", activate_pose_covariance_modifier_); + RCLCPP_INFO( + this->get_logger(), "aw_pose_covariance_modifier.enable set to : %d", + activate_pose_covariance_modifier_); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -213,11 +213,12 @@ void NDTScanMatcher::setParametersCallback( } } - if(activate_pose_covariance_modifier_ == 1){ + if (activate_pose_covariance_modifier_ == 1) { NDTScanMatcher::createTrustedSourcePoseSubscriber(); } } -void NDTScanMatcher::createTrustedSourcePoseSubscriber(){ +void NDTScanMatcher::createTrustedSourcePoseSubscriber() +{ trusted_source_pose_sub_ = this->create_subscription( "input_trusted_pose_topic", 100,