refactor(ndt scan matcher): apply static analysis

localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp

@@ -27,14 +27,14 @@ class DiagnosticsModule
 public:
   DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
   void clear();
-  void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg);
+  void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg);
   template <typename T>
-  void addKeyValue(const std::string & key, const T & value);
-  void updateLevelAndMessage(const int8_t level, const std::string & message);
+  void add_key_value(const std::string & key, const T & value);
+  void update_level_and_message(const int8_t level, const std::string & message);
   void publish(const rclcpp::Time & publish_time_stamp);
 
 private:
-  diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray(
+  [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array(
     const rclcpp::Time & publish_time_stamp) const;
 
   rclcpp::Clock::SharedPtr clock_;
@@ -44,17 +44,17 @@ class DiagnosticsModule
 };
 
 template <typename T>
-void DiagnosticsModule::addKeyValue(const std::string & key, const T & value)
+void DiagnosticsModule::add_key_value(const std::string & key, const T & value)
 {
   diagnostic_msgs::msg::KeyValue key_value;
   key_value.key = key;
   key_value.value = std::to_string(value);
-  addKeyValue(key_value);
+  add_key_value(key_value);
 }
 
 template <>
-void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value);
+void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value);
 template <>
-void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value);
+void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
 
 #endif  // NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

@@ -33,62 +33,62 @@ struct HyperParameters
 {
   struct Frame
   {
-    std::string base_frame;
-    std::string ndt_base_frame;
-    std::string map_frame;
-  } frame;
+    std::string base_frame{};
+    std::string ndt_base_frame{};
+    std::string map_frame{};
+  } frame{};
 
   struct SensorPoints
   {
-    double required_distance;
-  } sensor_points;
+    double required_distance{};
+  } sensor_points{};
 
-  pclomp::NdtParams ndt;
-  bool ndt_regularization_enable;
+  pclomp::NdtParams ndt{};
+  bool ndt_regularization_enable{};
 
   struct InitialPoseEstimation
   {
-    int64_t particles_num;
-    int64_t n_startup_trials;
-  } initial_pose_estimation;
+    int64_t particles_num{};
+    int64_t n_startup_trials{};
+  } initial_pose_estimation{};
 
   struct Validation
   {
-    double lidar_topic_timeout_sec;
-    double initial_pose_timeout_sec;
-    double initial_pose_distance_tolerance_m;
-    double critical_upper_bound_exe_time_ms;
-  } validation;
+    double lidar_topic_timeout_sec{};
+    double initial_pose_timeout_sec{};
+    double initial_pose_distance_tolerance_m{};
+    double critical_upper_bound_exe_time_ms{};
+  } validation{};
 
   struct ScoreEstimation
   {
-    ConvergedParamType converged_param_type;
-    double converged_param_transform_probability;
-    double converged_param_nearest_voxel_transformation_likelihood;
+    ConvergedParamType converged_param_type{};
+    double converged_param_transform_probability{};
+    double converged_param_nearest_voxel_transformation_likelihood{};
     struct NoGroundPoints
     {
-      bool enable;
-      double z_margin_for_ground_removal;
-    } no_ground_points;
-  } score_estimation;
+      bool enable{};
+      double z_margin_for_ground_removal{};
+    } no_ground_points{};
+  } score_estimation{};
 
   struct Covariance
   {
-    std::array<double, 36> output_pose_covariance;
+    std::array<double, 36> output_pose_covariance{};
 
     struct CovarianceEstimation
     {
-      bool enable;
-      std::vector<Eigen::Vector2d> initial_pose_offset_model;
-    } covariance_estimation;
-  } covariance;
+      bool enable{};
+      std::vector<Eigen::Vector2d> initial_pose_offset_model{};
+    } covariance_estimation{};
+  } covariance{};
 
   struct DynamicMapLoading
   {
-    double update_distance;
-    double map_radius;
-    double lidar_radius;
-  } dynamic_map_loading;
+    double update_distance{};
+    double map_radius{};
+    double lidar_radius{};
+  } dynamic_map_loading{};
 
 public:
   explicit HyperParameters(rclcpp::Node * node)

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -131,8 +131,6 @@ class NDTScanMatcher : public rclcpp::Node
 
   static int count_oscillation(const std::vector<geometry_msgs::msg::Pose> & result_pose_msg_array);
 
-  std::array<double, 36> rotate_covariance(
-    const std::array<double, 36> & src_covariance, const Eigen::Matrix3d & rotation) const;
   std::array<double, 36> estimate_covariance(
     const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
     const rclcpp::Time & sensor_ros_time);

localization/ndt_scan_matcher/src/diagnostics_module.cpp

@@ -41,7 +41,7 @@ void DiagnosticsModule::clear()
   diagnostics_status_msg_.message = "";
 }
 
-void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg)
+void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg)
 {
   auto it = std::find_if(
     std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
@@ -55,24 +55,24 @@ void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_v
 }
 
 template <>
-void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value)
+void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value)
 {
   diagnostic_msgs::msg::KeyValue key_value;
   key_value.key = key;
   key_value.value = value;
-  addKeyValue(key_value);
+  add_key_value(key_value);
 }
 
 template <>
-void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value)
+void DiagnosticsModule::add_key_value(const std::string & key, const bool & value)
 {
   diagnostic_msgs::msg::KeyValue key_value;
   key_value.key = key;
   key_value.value = value ? "True" : "False";
-  addKeyValue(key_value);
+  add_key_value(key_value);
 }
 
-void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message)
+void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message)
 {
   if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
     if (!diagnostics_status_msg_.message.empty()) {
@@ -87,10 +87,10 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str
 
 void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
 {
-  diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp));
+  diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp));
 }
 
-diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray(
+diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array(
   const rclcpp::Time & publish_time_stamp) const
 {
   diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -53,23 +53,23 @@ void MapUpdateModule::callback_timer(
   std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
 {
   // check is_activated
-  diagnostics_ptr->addKeyValue("is_activated", is_activated);
+  diagnostics_ptr->add_key_value("is_activated", is_activated);
   if (!is_activated) {
     std::stringstream message;
     message << "Node is not activated.";
-    diagnostics_ptr->updateLevelAndMessage(
+    diagnostics_ptr->update_level_and_message(
       diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
     return;
   }
 
   // check is_set_last_update_position
   const bool is_set_last_update_position = (position != std::nullopt);
-  diagnostics_ptr->addKeyValue("is_set_last_update_position", is_set_last_update_position);
+  diagnostics_ptr->add_key_value("is_set_last_update_position", is_set_last_update_position);
   if (!is_set_last_update_position) {
     std::stringstream message;
     message << "Cannot find the reference position for map update."
             << "Please check if the EKF odometry is provided to NDT.";
-    diagnostics_ptr->updateLevelAndMessage(
+    diagnostics_ptr->update_level_and_message(
       diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
     return;
   }
@@ -92,11 +92,11 @@ bool MapUpdateModule::should_update_map(
   const double distance = std::hypot(dx, dy);
 
   // check distance_last_update_position_to_current_position
-  diagnostics_ptr->addKeyValue("distance_last_update_position_to_current_position", distance);
+  diagnostics_ptr->add_key_value("distance_last_update_position_to_current_position", distance);
   if (distance + param_.lidar_radius > param_.map_radius) {
     std::stringstream message;
     message << "Dynamic map loading is not keeping up.";
-    diagnostics_ptr->updateLevelAndMessage(
+    diagnostics_ptr->update_level_and_message(
       diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
 
     // If the map does not keep up with the current position,
@@ -110,7 +110,7 @@ bool MapUpdateModule::should_update_map(
 void MapUpdateModule::update_map(
   const geometry_msgs::msg::Point & position, std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
 {
-  diagnostics_ptr->addKeyValue("is_need_rebuild", need_rebuild_);
+  diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_);
 
   // If the current position is super far from the previous loading position,
   // lock and rebuild ndt_ptr_
@@ -130,14 +130,14 @@ void MapUpdateModule::update_map(
     const bool updated = update_ndt(position, *ndt_ptr_, diagnostics_ptr);
 
     // check is_updated_map
-    diagnostics_ptr->addKeyValue("is_updated_map", updated);
+    diagnostics_ptr->add_key_value("is_updated_map", updated);
     if (!updated) {
       std::stringstream message;
       message
         << "update_ndt failed. If this happens with initial position estimation, make sure that"
         << "(1) the initial position matches the pcd map and (2) the map_loader is working "
            "properly.";
-      diagnostics_ptr->updateLevelAndMessage(
+      diagnostics_ptr->update_level_and_message(
         diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
       RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str());
       last_update_position_ = position;
@@ -157,7 +157,7 @@ void MapUpdateModule::update_map(
     const bool updated = update_ndt(position, *secondary_ndt_ptr_, diagnostics_ptr);
 
     // check is_updated_map
-    diagnostics_ptr->addKeyValue("is_updated_map", updated);
+    diagnostics_ptr->add_key_value("is_updated_map", updated);
     if (!updated) {
       last_update_position_ = position;
       return;
@@ -189,7 +189,7 @@ bool MapUpdateModule::update_ndt(
   const geometry_msgs::msg::Point & position, NdtType & ndt,
   std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
 {
-  diagnostics_ptr->addKeyValue("maps_size_before", ndt.getCurrentMapIDs().size());
+  diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size());
 
   auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
 
@@ -199,11 +199,11 @@ bool MapUpdateModule::update_ndt(
   request->cached_ids = ndt.getCurrentMapIDs();
 
   while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
-    diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false);
+    diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false);
 
     std::stringstream message;
     message << "Waiting for pcd loader service. Check the pointcloud_map_loader.";
-    diagnostics_ptr->updateLevelAndMessage(
+    diagnostics_ptr->update_level_and_message(
       diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
     return false;
   }
@@ -217,23 +217,23 @@ bool MapUpdateModule::update_ndt(
   while (status != std::future_status::ready) {
     // check is_succeed_call_pcd_loader
     if (!rclcpp::ok()) {
-      diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false);
+      diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false);
 
       std::stringstream message;
       message << "pcd_loader service is not working.";
-      diagnostics_ptr->updateLevelAndMessage(
+      diagnostics_ptr->update_level_and_message(
         diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
       return false;  // No update
     }
     status = result.wait_for(std::chrono::seconds(1));
   }
-  diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true);
+  diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true);
 
   auto & maps_to_add = result.get()->new_pointcloud_with_ids;
   auto & map_ids_to_remove = result.get()->ids_to_remove;
 
-  diagnostics_ptr->addKeyValue("maps_to_add_size", maps_to_add.size());
-  diagnostics_ptr->addKeyValue("maps_to_remove_size", map_ids_to_remove.size());
+  diagnostics_ptr->add_key_value("maps_to_add_size", maps_to_add.size());
+  diagnostics_ptr->add_key_value("maps_to_remove_size", map_ids_to_remove.size());
 
   if (maps_to_add.empty() && map_ids_to_remove.empty()) {
     return false;  // No update
@@ -261,9 +261,9 @@ bool MapUpdateModule::update_ndt(
   const auto duration_micro_sec =
     std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
-  diagnostics_ptr->addKeyValue("map_update_execution_time", exe_time);
-  diagnostics_ptr->addKeyValue("maps_size_after", ndt.getCurrentMapIDs().size());
-  diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true);
+  diagnostics_ptr->add_key_value("map_update_execution_time", exe_time);
+  diagnostics_ptr->add_key_value("maps_size_after", ndt.getCurrentMapIDs().size());
+  diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true);
   return true;  // Updated
 }