refactor(pose_initializer): apply static analysis (#7355)

* apply clang-tidy and cpplint Signed-off-by: Yamato Ando <[email protected]> * apply cppcheck Signed-off-by: Yamato Ando <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jun 10, 2024 · badc3e7 · badc3e7
1 parent a78822b
commit badc3e7
Show file tree

Hide file tree

Showing 8 changed files with 36 additions and 34 deletions.
diff --git a/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp
@@ -40,7 +40,7 @@ template <class NodeT>
 std::array<double, 36> get_covariance_parameter(NodeT * node, const std::string & name)
 {
   const auto parameter = node->template declare_parameter<std::vector<double>>(name);
-  std::array<double, 36> covariance;
+  std::array<double, 36> covariance{};
   copy_vector_to_array(parameter, covariance);
   return covariance;
 }

diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp
@@ -19,13 +19,18 @@
 
 #include <memory>
 
-GnssModule::GnssModule(rclcpp::Node * node) : fitter_(node)
+GnssModule::GnssModule(rclcpp::Node * node)
+: fitter_(node),
+  clock_(node->get_clock()),
+  timeout_(node->declare_parameter<double>("gnss_pose_timeout"))
 {
   sub_gnss_pose_ = node->create_subscription<PoseWithCovarianceStamped>(
-    "gnss_pose_cov", 1, [this](PoseWithCovarianceStamped::ConstSharedPtr msg) { pose_ = msg; });
+    "gnss_pose_cov", 1, std::bind(&GnssModule::on_pose, this, std::placeholders::_1));
+}
 
-  clock_ = node->get_clock();
-  timeout_ = node->declare_parameter<double>("gnss_pose_timeout");
+void GnssModule::on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg)
+{
+  pose_ = msg;
 }
 
 geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose()

diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp
@@ -30,6 +30,8 @@ class GnssModule
   PoseWithCovarianceStamped get_pose();
 
 private:
+  void on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg);
+
   map_height_fitter::MapHeightFitter fitter_;
   rclcpp::Clock::SharedPtr clock_;
   rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_gnss_pose_;

diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp
@@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException;
 using Initialize = localization_interface::Initialize;
 using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
 
-NdtModule::NdtModule(rclcpp::Node * node) : logger_(node->get_logger())
+NdtModule::NdtModule(rclcpp::Node * node)
+: logger_(node->get_logger()), cli_align_(node->create_client<RequestPoseAlignment>("ndt_align"))
 {
-  cli_align_ = node->create_client<RequestPoseAlignment>("ndt_align");
 }
 
 PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped & pose)

diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp
@@ -67,28 +67,24 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options)
       throw std::invalid_argument(
         "Could not set user defined initial pose. The size of initial_pose is " +
         std::to_string(initial_pose_array.size()) + ". It must be 7.");
-    } else if (
+    }
+    if (
       std::abs(initial_pose_array[3]) < 1e-6 && std::abs(initial_pose_array[4]) < 1e-6 &&
       std::abs(initial_pose_array[5]) < 1e-6 && std::abs(initial_pose_array[6]) < 1e-6) {
       throw std::invalid_argument("Input quaternion is invalid. All elements are close to zero.");
-    } else {
-      geometry_msgs::msg::Pose initial_pose;
-      initial_pose.position.x = initial_pose_array[0];
-      initial_pose.position.y = initial_pose_array[1];
-      initial_pose.position.z = initial_pose_array[2];
-      initial_pose.orientation.x = initial_pose_array[3];
-      initial_pose.orientation.y = initial_pose_array[4];
-      initial_pose.orientation.z = initial_pose_array[5];
-      initial_pose.orientation.w = initial_pose_array[6];
-
-      set_user_defined_initial_pose(initial_pose, true);
     }
-  }
-}
 
-PoseInitializer::~PoseInitializer()
-{
-  // to delete gnss module
+    geometry_msgs::msg::Pose initial_pose;
+    initial_pose.position.x = initial_pose_array[0];
+    initial_pose.position.y = initial_pose_array[1];
+    initial_pose.position.z = initial_pose_array[2];
+    initial_pose.orientation.x = initial_pose_array[3];
+    initial_pose.orientation.y = initial_pose_array[4];
+    initial_pose.orientation.z = initial_pose_array[5];
+    initial_pose.orientation.w = initial_pose_array[6];
+
+    set_user_defined_initial_pose(initial_pose, true);
+  }
 }
 
 void PoseInitializer::change_state(State::Message::_state_type state)
@@ -175,7 +171,7 @@ void PoseInitializer::on_initialize(
         std::stringstream message;
         message << "No input pose_with_covariance. If you want to use DIRECT method, please input "
                    "pose_with_covariance.";
-        RCLCPP_ERROR(get_logger(), message.str().c_str());
+        RCLCPP_ERROR_STREAM(get_logger(), message.str());
         throw ServiceException(
           autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str());
       }
@@ -186,7 +182,7 @@ void PoseInitializer::on_initialize(
     } else {
       std::stringstream message;
       message << "Unknown method type (=" << std::to_string(req->method) << ")";
-      RCLCPP_ERROR(get_logger(), message.str().c_str());
+      RCLCPP_ERROR_STREAM(get_logger(), message.str());
       throw ServiceException(
         autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str());
     }

diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp
@@ -35,7 +35,6 @@ class PoseInitializer : public rclcpp::Node
 {
 public:
   explicit PoseInitializer(const rclcpp::NodeOptions & options);
-  ~PoseInitializer();
 
 private:
   using ServiceException = component_interface_utils::ServiceException;
@@ -48,8 +47,8 @@ class PoseInitializer : public rclcpp::Node
   component_interface_utils::Publisher<State>::SharedPtr pub_state_;
   component_interface_utils::Service<Initialize>::SharedPtr srv_initialize_;
   State::Message state_;
-  std::array<double, 36> output_pose_covariance_;
-  std::array<double, 36> gnss_particle_covariance_;
+  std::array<double, 36> output_pose_covariance_{};
+  std::array<double, 36> gnss_particle_covariance_{};
   std::unique_ptr<GnssModule> gnss_;
   std::unique_ptr<NdtModule> ndt_;
   std::unique_ptr<YabLocModule> yabloc_;

diff --git a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp
@@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException;
 using Initialize = localization_interface::Initialize;
 using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
 
-YabLocModule::YabLocModule(rclcpp::Node * node) : logger_(node->get_logger())
+YabLocModule::YabLocModule(rclcpp::Node * node)
+: logger_(node->get_logger()), cli_align_(node->create_client<RequestPoseAlignment>("yabloc_align"))
 {
-  cli_align_ = node->create_client<RequestPoseAlignment>("yabloc_align");
 }
 
 PoseWithCovarianceStamped YabLocModule::align_pose(const PoseWithCovarianceStamped & pose)

diff --git a/localization/pose_initializer/test/test_copy_vector_to_array.cpp b/localization/pose_initializer/test/test_copy_vector_to_array.cpp
@@ -19,7 +19,7 @@
 TEST(CopyVectorToArray, CopyAllElements)
 {
   const std::vector<int> vector{0, 1, 2, 3, 4};
-  std::array<int, 5> array;
+  std::array<int, 5> array{};
   copy_vector_to_array<int, 5>(vector, array);
   EXPECT_THAT(array, testing::ElementsAre(0, 1, 2, 3, 4));
 }
@@ -28,15 +28,15 @@ TEST(CopyVectorToArray, CopyZeroElements)
 {
   const std::vector<int> vector{};
   // just confirm that this works
-  std::array<int, 0> array;
+  std::array<int, 0> array{};
   copy_vector_to_array<int, 0>(vector, array);
 }
 
 TEST(CopyVectorToArray, ThrowsInvalidArgumentIfMoreElementsExpected)
 {
   auto f = [] {
     const std::vector<int> vector{0, 1, 2, 3, 4};
-    std::array<int, 6> array;
+    std::array<int, 6> array{};
     copy_vector_to_array<int, 6>(vector, array);
   };