linter

src/perception/tracking/dets_2d_3d/include/cluster.hpp

@@ -1,34 +1,33 @@
 #pragma once
 
-#include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 #include <vision_msgs/msg/detection3_d_array.hpp>
 
 #include <vector>
 
-class Cluster
-{
+class Cluster {
 public:
-    // makes a cloud from all points `in_cloud_ptr` that are indexed by `in_cluster_indices`
-    Cluster(
-        const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud_ptr, 
-        const std::vector<int>& in_cluster_indices);
+  // makes a cloud from all points `in_cloud_ptr` that are indexed by
+  // `in_cluster_indices`
+  Cluster(const pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud_ptr,
+          const std::vector<int> &in_cluster_indices);
 
-    pcl::PointXYZ getCentroid() { return centroid_; }
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr getCloud() { return cloud_; }
-    vision_msgs::msg::BoundingBox3D getBoundingBox();
+  pcl::PointXYZ getCentroid() { return centroid_; }
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr getCloud() { return cloud_; }
+  vision_msgs::msg::BoundingBox3D getBoundingBox();
 
-    bool isValid();
-    int size() { return cloud_->size(); }
+  bool isValid();
+  int size() { return cloud_->size(); }
 
 private:
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_;
-    pcl::PointXYZ centroid_;
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_;
+  pcl::PointXYZ centroid_;
 
-    pcl::PointXYZ max_point_;
-    pcl::PointXYZ min_point_;
+  pcl::PointXYZ max_point_;
+  pcl::PointXYZ min_point_;
 
-    static int color_index;
-    static std::vector<std::vector<int>> colors;
+  static int color_index;
+  static std::vector<std::vector<int>> colors;
 };

src/perception/tracking/dets_2d_3d/include/dets_2d_3d_node.hpp

@@ -10,11 +10,11 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
 #include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
 
-#include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 #include "cluster.hpp"
 #include "projection_utils.hpp"
@@ -23,65 +23,67 @@
 
 struct ClusteringParams;
 
-class TrackingNode : public rclcpp::Node
-{
+class TrackingNode : public rclcpp::Node {
 public:
-    TrackingNode();
+  TrackingNode();
 
 private:
-    std::map<std::string, ClusteringParams> clusteringParams;
-
-    // lidar and camera frame names
-    std::string lidarFrame_;
-    std::string cameraFrame_;
-
-    // synchronization utils
-    std::mutex lidarCloud_mutex_;
-
-    // camera information cached
-    bool transformInited_;
-    geometry_msgs::msg::TransformStamped transform_;
-    sensor_msgs::msg::CameraInfo::SharedPtr camInfo_;
-    pcl::PointCloud<pcl::PointXYZ>::Ptr lidarCloud_;
-
-    // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection boxes
-    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camInfo_subscriber_;
-    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_subscriber_;
-    rclcpp::Subscription<vision_msgs::msg::Detection2DArray>::SharedPtr det_subscriber_;
-
-    // publish the 3d detections
-    rclcpp::Publisher<vision_msgs::msg::Detection3DArray>::SharedPtr det3d_publisher_;
-    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
-    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_publisher_;
-    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_publisher2_;
-
-    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-
-    void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
-    void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
-    void receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg);
-
-    int highestIOUScoredBBox(
-        const std::vector<vision_msgs::msg::BoundingBox3D> bboxes,
-        const vision_msgs::msg::BoundingBox2D& detBBox,
-        const std::vector<std::shared_ptr<Cluster>>& clusters);
-    double overlapBoundingBox(
-        const vision_msgs::msg::BoundingBox2D& bboxA, 
-        const vision_msgs::msg::BoundingBox2D& bboxB);
-    double iouScore(
-        const vision_msgs::msg::BoundingBox2D& bboxA, 
-        const vision_msgs::msg::BoundingBox2D& bboxB);
-
-    std::map<std::string, ClusteringParams> initializeClusteringParams(
-        const std::map<std::string, rclcpp::Parameter>& clustering_params_map);
-
-    template <typename T>
-    T getDefaultOrValue(std::map<std::string, T> m, std::string key)
-    {
-        if (m.find(key) == m.end())
-            return m[key];
-        return m["default"];
-    }
-
+  std::map<std::string, ClusteringParams> clusteringParams;
+
+  // lidar and camera frame names
+  std::string lidarFrame_;
+  std::string cameraFrame_;
+
+  // synchronization utils
+  std::mutex lidarCloud_mutex_;
+
+  // camera information cached
+  bool transformInited_;
+  geometry_msgs::msg::TransformStamped transform_;
+  sensor_msgs::msg::CameraInfo::SharedPtr camInfo_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr lidarCloud_;
+
+  // subscribers to camera intrinsics, lidar pts, camera feed, 2d detection
+  // boxes
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr
+      camInfo_subscriber_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
+      lidar_subscriber_;
+  rclcpp::Subscription<vision_msgs::msg::Detection2DArray>::SharedPtr
+      det_subscriber_;
+
+  // publish the 3d detections
+  rclcpp::Publisher<vision_msgs::msg::Detection3DArray>::SharedPtr
+      det3d_publisher_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+      marker_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_publisher2_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  void readCameraInfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
+  void receiveLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+  void
+  receiveDetections(const vision_msgs::msg::Detection2DArray::SharedPtr msg);
+
+  int highestIOUScoredBBox(
+      const std::vector<vision_msgs::msg::BoundingBox3D> bboxes,
+      const vision_msgs::msg::BoundingBox2D &detBBox,
+      const std::vector<std::shared_ptr<Cluster>> &clusters);
+  double overlapBoundingBox(const vision_msgs::msg::BoundingBox2D &bboxA,
+                            const vision_msgs::msg::BoundingBox2D &bboxB);
+  double iouScore(const vision_msgs::msg::BoundingBox2D &bboxA,
+                  const vision_msgs::msg::BoundingBox2D &bboxB);
+
+  std::map<std::string, ClusteringParams> initializeClusteringParams(
+      const std::map<std::string, rclcpp::Parameter> &clustering_params_map);
+
+  template <typename T>
+  T getDefaultOrValue(std::map<std::string, T> m, std::string key) {
+    if (m.find(key) == m.end())
+      return m[key];
+    return m["default"];
+  }
 };

src/perception/tracking/dets_2d_3d/include/projection_utils.hpp

@@ -2,72 +2,75 @@
 
 #include "cluster.hpp"
 
-#include <pcl/point_types.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 #include <vision_msgs/msg/bounding_box2_d.hpp>
 #include <vision_msgs/msg/bounding_box3_d.hpp>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-#include <geometry_msgs/msg/point.hpp>
 
-#include <vector>
 #include <optional>
+#include <vector>
 
-struct ClusteringParams 
-{
-    // segment by distance
-    std::vector<double> clustering_distances;
-    // Map of the nearest neighbor distance threshold for each segment, for each detection
-    std::vector<double> clustering_thresholds;
+struct ClusteringParams {
+  // segment by distance
+  std::vector<double> clustering_distances;
+  // Map of the nearest neighbor distance threshold for each segment, for each
+  // detection
+  std::vector<double> clustering_thresholds;
 
-    double cluster_size_min;
-    double cluster_size_max;
-    double cluster_merge_threshold;
+  double cluster_size_min;
+  double cluster_size_max;
+  double cluster_merge_threshold;
 };
 
 // main helper functions used by the node -- should all be static
-class ProjectionUtils
-{
+class ProjectionUtils {
 public:
-        static void pointsInBbox(
-                const pcl::PointCloud<pcl::PointXYZ>::Ptr& inlierCloud,
-                const pcl::PointCloud<pcl::PointXYZ>::Ptr& lidarCloud, 
-                const std::vector<geometry_msgs::msg::Point>& projs2d,
-                const vision_msgs::msg::BoundingBox2D& bbox);
+  static void
+  pointsInBbox(const pcl::PointCloud<pcl::PointXYZ>::Ptr &inlierCloud,
+               const pcl::PointCloud<pcl::PointXYZ>::Ptr &lidarCloud,
+               const std::vector<geometry_msgs::msg::Point> &projs2d,
+               const vision_msgs::msg::BoundingBox2D &bbox);
 
-        // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to camera frame
-        static std::optional<geometry_msgs::msg::Point> projectLidarToCamera(
-                const geometry_msgs::msg::TransformStamped& transform,
-                const std::array<double, 12>& P, 
-                const pcl::PointXYZ& pt);
+  // P : projection matrix, pt : 3D lidar pt, transform: transform from lidar to
+  // camera frame
+  static std::optional<geometry_msgs::msg::Point>
+  projectLidarToCamera(const geometry_msgs::msg::TransformStamped &transform,
+                       const std::array<double, 12> &P,
+                       const pcl::PointXYZ &pt);
 
-        static bool isPointInBbox(
-                const geometry_msgs::msg::Point& pt, 
-                const vision_msgs::msg::BoundingBox2D& bbox);
+  static bool isPointInBbox(const geometry_msgs::msg::Point &pt,
+                            const vision_msgs::msg::BoundingBox2D &bbox);
 
-        static void removeFloor(
-                const pcl::PointCloud<pcl::PointXYZ>::Ptr& lidarCloud, 
-                const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered);
+  static void
+  removeFloor(const pcl::PointCloud<pcl::PointXYZ>::Ptr &lidarCloud,
+              const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_filtered);
 
-        static std::pair<std::vector<std::shared_ptr<Cluster>>, std::vector<vision_msgs::msg::BoundingBox3D>> getClusteredBBoxes(
-                const pcl::PointCloud<pcl::PointXYZ>::Ptr& lidarCloud, const ClusteringParams& clusteringParams);
+  static std::pair<std::vector<std::shared_ptr<Cluster>>,
+                   std::vector<vision_msgs::msg::BoundingBox3D>>
+  getClusteredBBoxes(const pcl::PointCloud<pcl::PointXYZ>::Ptr &lidarCloud,
+                     const ClusteringParams &clusteringParams);
 
 private:
+  static std::vector<std::shared_ptr<Cluster>>
+  clusterAndColor(const pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud_ptr,
+                  double in_max_cluster_distance, double cluster_size_min,
+                  double cluster_size_max);
 
-        static std::vector<std::shared_ptr<Cluster>> clusterAndColor(
-                const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud_ptr, 
-                double in_max_cluster_distance, double cluster_size_min, double cluster_size_max);
-
-        static void checkClusterMerge(
-                size_t in_cluster_id, const std::vector<std::shared_ptr<Cluster>> &in_clusters,
-                std::vector<bool> &in_out_visited_clusters,
-                std::vector<size_t> &out_merge_indices, double in_merge_threshold);
+  static void
+  checkClusterMerge(size_t in_cluster_id,
+                    const std::vector<std::shared_ptr<Cluster>> &in_clusters,
+                    std::vector<bool> &in_out_visited_clusters,
+                    std::vector<size_t> &out_merge_indices,
+                    double in_merge_threshold);
 
-        static std::shared_ptr<Cluster> mergeClusters(
-                const std::vector<std::shared_ptr<Cluster>>& in_clusters,
-                const std::vector<size_t>& in_merge_indices,
-                std::vector<bool>& in_out_merged_clusters);
+  static std::shared_ptr<Cluster>
+  mergeClusters(const std::vector<std::shared_ptr<Cluster>> &in_clusters,
+                const std::vector<size_t> &in_merge_indices,
+                std::vector<bool> &in_out_merged_clusters);
 
-        static std::vector<std::shared_ptr<Cluster>> checkAllForMerge(
-                const std::vector<std::shared_ptr<Cluster>>& in_clusters, 
-                float in_merge_threshold);
+  static std::vector<std::shared_ptr<Cluster>>
+  checkAllForMerge(const std::vector<std::shared_ptr<Cluster>> &in_clusters,
+                   float in_merge_threshold);
 };