style(pre-commit): autofix

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_loader.hpp

@@ -1,5 +1,5 @@
-#ifndef LOC_MAP_LOADER_HPP_
-#define LOC_MAP_LOADER_HPP_
+#ifndef NDT_SCAN_MATCHER__MAP_LOADER_HPP_
+#define NDT_SCAN_MATCHER__MAP_LOADER_HPP_
 
 // Copyright 2022 The Autoware Contributors
 //
@@ -15,40 +15,41 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "utils.hpp"
 
-#include <map>
-#include <string>
-#include <vector>
-#include <future>
-#include <chrono>
-#include <mutex>
-#include <queue>
-
+#include <ndt_scan_matcher/metadata.hpp>
 #include <rclcpp/rclcpp.hpp>
+
 #include <pcl/common/common.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
-#include <ndt_scan_matcher/metadata.hpp>
-#include "utils.hpp"
+#include <chrono>
+#include <future>
+#include <map>
+#include <mutex>
+#include <queue>
+#include <string>
+#include <vector>
 
 namespace loc
 {
 
 template <typename PointT>
 class MapLoader
 {
-    typedef pcl::PointCloud<PointT> PclCloudType;
-    typedef typename PclCloudType::Ptr PclCloudPtr;
+  typedef pcl::PointCloud<PointT> PclCloudType;
+  typedef typename PclCloudType::Ptr PclCloudPtr;
 
 public:
-  MapLoader() {
+  MapLoader()
+  {
     thread_num_ = 1;
 
-    setThreadNum(4); 
+    setThreadNum(4);
   }
 
   explicit MapLoader(std::shared_ptr<PCDMetadata> metadata);
@@ -59,16 +60,16 @@ class MapLoader
     metadata_->import(metadata_path, pcd_dir);
   }
 
-  // Retrieve a map whose key is segment indices and value is a pointer to 
+  // Retrieve a map whose key is segment indices and value is a pointer to
   // the point cloud loaded from the segment PCD
-  void load(float pos_x, float pos_y, float radius, const std::vector<std::string> & cached_ids,
-    std::map<std::string, PclCloudPtr> & pcd_to_add,
-    std::set<std::string> & ids_to_remove);
+  void load(
+    float pos_x, float pos_y, float radius, const std::vector<std::string> & cached_ids,
+    std::map<std::string, PclCloudPtr> & pcd_to_add, std::set<std::string> & ids_to_remove);
 
   // Find the names of PCDs to be added/removed but not load them from files
-  void query(float pos_x, float pos_y, float radius, const std::vector<std::string> & cached_ids,
-    std::map<std::string, std::string> & pcd_to_add,
-    std::set<std::string> & ids_to_remove);
+  void query(
+    float pos_x, float pos_y, float radius, const std::vector<std::string> & cached_ids,
+    std::map<std::string, std::string> & pcd_to_add, std::set<std::string> & ids_to_remove);
 
   // Warper around loadPCDFile
   int load(const std::string & path, PclCloudType & cloud);
@@ -77,8 +78,7 @@ class MapLoader
   {
     sync();
 
-    if (thread_num > 1)
-    {
+    if (thread_num > 1) {
       // One more thread for the load manager
       thread_num += 1;
       thread_num_ = thread_num;
@@ -87,14 +87,14 @@ class MapLoader
   }
 
   // Wait for all running threads to finish
-  inline void sync() {
-    if (load_manager_fut_.valid())
-    {
+  inline void sync()
+  {
+    if (load_manager_fut_.valid()) {
       load_manager_fut_.wait();
     }
 
-    for(auto &tf : thread_futs_) {
-      if(tf.valid()) {
+    for (auto & tf : thread_futs_) {
+      if (tf.valid()) {
         tf.wait();
       }
     }
@@ -103,39 +103,38 @@ class MapLoader
   }
 
   // Parallel load but pcd files come in stream
-  void parallel_load_setup(float x, float y, float radius, const std::vector<std::string> & cached_ids);
-
+  void parallel_load_setup(
+    float x, float y, float radius, const std::vector<std::string> & cached_ids);
+
   // Return false if no more pcd is loaded, true otherwise
   bool get_next_loaded_pcd(std::string & map_id, PclCloudPtr & loaded_pcd);
 
-  std::set<std::string> get_pcd_id_to_remove()
-  {
-    return pcd_to_remove_;
-  }
-
+  std::set<std::string> get_pcd_id_to_remove() { return pcd_to_remove_; }
+
   // Clear the current stream
   void parallel_load_clear();
-private:
 
+private:
   bool loadThread(const std::string & map_id, const std::string & path);
   bool loadManagerThread();
 
   // Return the index of an idle thread, which is not running any
   // job, or has already finished its job and waiting for a join.
-  inline int get_idle_tid() {
+  inline int get_idle_tid()
+  {
     int tid = (last_check_tid_ == thread_num_ - 1) ? 0 : last_check_tid_ + 1;
     std::chrono::microseconds span(50);
 
     // Loop until an idle thread is found
-    while(true) {
+    while (true) {
       // Return immediately if a thread that has not been given a job is found
-      if(!thread_futs_[tid].valid()) {
+      if (!thread_futs_[tid].valid()) {
         last_check_tid_ = tid;
         return tid;
       }
 
       // If no such thread is found, wait for the current thread to finish its job
-      if(thread_futs_[tid].wait_for(span) == std::future_status::ready) {
+      if (thread_futs_[tid].wait_for(span) == std::future_status::ready) {
         last_check_tid_ = tid;
         return tid;
       }
@@ -158,6 +157,6 @@ class MapLoader
   size_t loaded_counter_;
 };
 
-}
+}  // namespace loc
 
-#endif  // LOC_MAP_LOADER_HPP_
+#endif  // NDT_SCAN_MATCHER__MAP_LOADER_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -20,15 +20,16 @@
 #include "ndt_scan_matcher/hyper_parameters.hpp"
 #include "ndt_scan_matcher/particle.hpp"
 
+#include <ndt_scan_matcher/map_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 #include <tier4_autoware_utils/transform/transforms.hpp>
 
 #include <autoware_map_msgs/srv/get_differential_point_cloud_map.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <fmt/format.h>
 #include <multigrid_pclomp/multigrid_ndt_omp.h>
@@ -41,8 +42,6 @@
 #include <thread>
 #include <vector>
 
-#include <ndt_scan_matcher/map_loader.hpp>
-
 class MapUpdateModule
 {
   using PointSource = pcl::PointXYZ;

localization/ndt_scan_matcher/include/ndt_scan_matcher/metadata.hpp

@@ -12,35 +12,35 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LOC_PCD_METADATA_HPP_
-#define LOC_PCD_METADATA_HPP_
+#ifndef NDT_SCAN_MATCHER__METADATA_HPP_
+#define NDT_SCAN_MATCHER__METADATA_HPP_
+
+#include <yaml-cpp/yaml.h>
 
-#include <iostream>
 #include <filesystem>
-#include <string>
-#include <set>
 #include <fstream>
-
-#include <yaml-cpp/yaml.h>
+#include <iostream>
+#include <set>
+#include <string>
 
 namespace loc
 {
 
 struct SegmentIndex
 {
-    int x, y;
+  int x, y;
 
-    SegmentIndex() {}
-    SegmentIndex(int in_x, int in_y) : x(in_x), y(in_y) {}
-    SegmentIndex(const SegmentIndex& other) : x(other.x), y(other.y) {}
-    SegmentIndex(SegmentIndex&& other) : x(other.x), y(other.y) {}
+  SegmentIndex() {}
+  SegmentIndex(int in_x, int in_y) : x(in_x), y(in_y) {}
+  SegmentIndex(const SegmentIndex & other) : x(other.x), y(other.y) {}
+  SegmentIndex(SegmentIndex && other) : x(other.x), y(other.y) {}
 
-    SegmentIndex& operator=(const SegmentIndex& other);
-    SegmentIndex& operator=(SegmentIndex&& other);
+  SegmentIndex & operator=(const SegmentIndex & other);
+  SegmentIndex & operator=(SegmentIndex && other);
 
-    bool operator<(const SegmentIndex& other) const;
+  bool operator<(const SegmentIndex & other) const;
 
-    std::string to_string() const;
+  std::string to_string() const;
 };
 
 struct PCDMetadata
@@ -57,48 +57,44 @@ struct PCDMetadata
   PCDMetadata();
 
   // Import data from a metadata file
-  void import(const std::string& pcd_metadata_path, const std::string& pcd_dir);
+  void import(const std::string & pcd_metadata_path, const std::string & pcd_dir);
 
   // Utility function to quickly convert coordinate to segment index
   // Notice that the SegmentIndex is the BOUNDARY coordinate in integer of the segment
   // It is NOT the segmentation index (i.e. coordinate / resolution)
   SegmentIndex coorToSegmentIndex(float x, float y) const;
 
-  void segmentIndexToBoundary(const SegmentIndex & sid, float & min_x, float & min_y, float & max_x, float & max_y);
+  void segmentIndexToBoundary(
+    const SegmentIndex & sid, float & min_x, float & min_y, float & max_x, float & max_y);
 
-  std::map<SegmentIndex, std::string>::iterator find(const SegmentIndex& key);
+  std::map<SegmentIndex, std::string>::iterator find(const SegmentIndex & key);
 
   // Relay endpoints of segment list to outside
   auto begin() -> decltype(segment_list_.begin());
   auto end() -> decltype(segment_list_.end());
 
-  size_t size()
-  {
-    return segment_list_.size();
-  }
+  size_t size() { return segment_list_.size(); }
 
 private:
-    // Safely find a key from a YAML node, and save its value to val
-    template <typename T>
-    bool safeLoad(const YAML::Node& node, const std::string& key, T& val)
-    {
-        bool retval = true;
-
-        try
-        {
-            auto val_node = node[key];
-            val = val_node.as<T>();
-        }
-        catch (YAML::Exception & e)
-        {
-            std::cerr << "Failed to retrieve a value at key " << key << ". Error: " << e.what() << std::endl;
-            retval = false;
-        }
-
-        return retval;
+  // Safely find a key from a YAML node, and save its value to val
+  template <typename T>
+  bool safeLoad(const YAML::Node & node, const std::string & key, T & val)
+  {
+    bool retval = true;
+
+    try {
+      auto val_node = node[key];
+      val = val_node.as<T>();
+    } catch (YAML::Exception & e) {
+      std::cerr << "Failed to retrieve a value at key " << key << ". Error: " << e.what()
+                << std::endl;
+      retval = false;
     }
+
+    return retval;
+  }
 };
 
-}
+}  // namespace loc
 
-#endif
+#endif  // NDT_SCAN_MATCHER__METADATA_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/utils.hpp

@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LOC_UTILS_HPP_
-#define LOC_UTILS_HPP_
+#ifndef NDT_SCAN_MATCHER__UTILS_HPP_
+#define NDT_SCAN_MATCHER__UTILS_HPP_
+
+#include <ndt_scan_matcher/metadata.hpp>
 
 #include <pcl/common/common.h>
 #include <yaml-cpp/yaml.h>
@@ -22,16 +24,16 @@
 #include <string>
 #include <vector>
 
-#include <ndt_scan_matcher/metadata.hpp>
-
 namespace loc
 {
 
 bool cylinderAndBoxOverlapExists(
   const double center_x, const double center_y, const double radius,
   const pcl::PointXYZ position_min, const pcl::PointXYZ position_max);
 
-void queryContainedSegmentIdx(float center_x, float center_y, float radius, const PCDMetadata& m, std::list<SegmentIndex>& map_id);
-}
+void queryContainedSegmentIdx(
+  float center_x, float center_y, float radius, const PCDMetadata & m,
+  std::list<SegmentIndex> & map_id);
+}  // namespace loc
 
-#endif  // LOC_UTILS_HPP_
+#endif  // NDT_SCAN_MATCHER__UTILS_HPP_