added headerfile for clustering and cleaned up code

src/target_tracking/CMakeLists.txt

@@ -38,11 +38,17 @@ ament_target_dependencies(cloud_preprocessing_node
 
 # Node for clustering
 add_executable(cloud_clustering_node
-      src/cloud_clustering.cpp
+      src/cloud_clustering_node.cpp
 )
 
+add_library(cloud_clustering SHARED
+  src/cloud_clustering.cpp)
+target_compile_definitions(cloud_clustering
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+
+
 # Dependencies for clustering
-ament_target_dependencies(cloud_clustering_node
+ament_target_dependencies(cloud_clustering
   rclcpp
   sensor_msgs
   visualization_msgs 
@@ -51,7 +57,10 @@ ament_target_dependencies(cloud_clustering_node
   OpenCV
 )
 
-#target_link_libraries(target_tracking_node ${PCL_LIBRARIES})
+rclcpp_components_register_nodes(cloud_clustering "cloud_clustering::CloudClustering")
+set(node_plugins "${node_plugins}cloud_clustering::CloudClustering;$<TARGET_FILE:cloud_clustering>\n")
+
+target_link_libraries(cloud_clustering_node cloud_clustering)
 
 # Install target
 install(TARGETS
@@ -60,6 +69,13 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(TARGETS
+    cloud_clustering
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION lib/${PACKAGE_NAME}
+)
+
 # Install launchfile
 install(
   DIRECTORY launch/

src/target_tracking/include/cloud_clustering.hpp

@@ -0,0 +1,100 @@
+#ifndef CLOUD_CLUSTERING_HPP
+#define CLOUD_CLUSTERING_HPP
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <geometry_msgs/msg/point.hpp>
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#include "opencv2/video/tracking.hpp"
+
+#include "std_msgs/msg/float32_multi_array.hpp"
+#include "std_msgs/msg/int32_multi_array.hpp"
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+namespace cloud_clustering
+{
+    // KF init
+    int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
+    int measDim = 2;  // [z_x,z_y,z_w,z_h]
+    int ctrlDim = 0;
+
+    std::string frame_id;
+    std::string topic_in;
+
+    float z_dim_scale;
+    float cluster_tolerance;
+    int min_cluster_size;
+    int max_cluster_size;
+
+    cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
+
+    std::vector<geometry_msgs::msg::Point> prevClusterCenters;
+
+    cv::Mat state(stateDim, 1, CV_32F);
+    cv::Mat_<float> measurement(2, 1);
+
+    std::vector<int> objID; // Output of the data association using KF
+                            // measurement.setTo(Scalar(0));
+
+    bool firstFrame = true;
+    /*
+    cv::Mat state(stateDim, 1, CV_32F);
+    cv::Mat_<float> measurement(2, 1);
+    */
+
+    class CloudClustering : public rclcpp::Node
+    {
+    public:
+        CloudClustering(
+            const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+        CloudClustering(
+            const std::string &name_space,
+            const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+
+    private:
+        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub;
+        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markerPub;
+        rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr objID_pub;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster0;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster1;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster2;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster3;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster4;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster5;
+
+        std::vector<geometry_msgs::msg::Point> prevClusterCenters;
+
+        std::vector<int> objID; // Output of the data association using KF
+                                // measurement.setTo(Scalar(0));
+
+        bool firstFrame = true;
+        rclcpp::Clock::SharedPtr clock_;
+        std::string frame_id;
+        std::string filtered_cloud;
+
+        double euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2);
+        std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat);
+        void KFT(const std_msgs::msg::Float32MultiArray ccs);
+        void publish_cloud(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
+                           pcl::PointCloud<pcl::PointXYZI>::Ptr cluster);
+
+        void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
+                         std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> &cluster_vec,
+                         std::vector<pcl::PointXYZI> &clusterCentroids);
+        void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input);
+    };
+
+}
+#endif // CLOUD_CLUSTERING_HPP

src/target_tracking/src/cloud_clustering_node.cpp

@@ -0,0 +1,17 @@
+#include "cloud_clustering.hpp"
+
+int main(int argc, char * argv[])
+{
+    // Force flush of the stdout buffer.
+    // This ensures a correct sync of all prints
+    // even when executed simultaneously within a launch file.
+    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+    
+    rclcpp::init(argc, argv);
+    const rclcpp::NodeOptions options;
+
+    rclcpp::spin(std::make_shared<cloud_clustering::CloudClustering>(options));
+    
+    rclcpp::shutdown();
+    return 0;
+}