From f53702732974af765dbb750da532c18ed648bdbd Mon Sep 17 00:00:00 2001 From: Liam Date: Sat, 30 Aug 2025 14:50:43 +0200 Subject: [PATCH] added headerfile for clustering and cleaned up code --- src/target_tracking/CMakeLists.txt | 22 +- .../include/cloud_clustering.hpp | 100 ++ src/target_tracking/src/cloud_clustering.cpp | 1205 ++++++++--------- .../src/cloud_clustering_node.cpp | 17 + 4 files changed, 719 insertions(+), 625 deletions(-) create mode 100644 src/target_tracking/include/cloud_clustering.hpp create mode 100644 src/target_tracking/src/cloud_clustering_node.cpp diff --git a/src/target_tracking/CMakeLists.txt b/src/target_tracking/CMakeLists.txt index 751b007..430f2e4 100644 --- a/src/target_tracking/CMakeLists.txt +++ b/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;$\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/ diff --git a/src/target_tracking/include/cloud_clustering.hpp b/src/target_tracking/include/cloud_clustering.hpp new file mode 100644 index 0000000..9f61cdd --- /dev/null +++ b/src/target_tracking/include/cloud_clustering.hpp @@ -0,0 +1,100 @@ +#ifndef CLOUD_CLUSTERING_HPP +#define CLOUD_CLUSTERING_HPP + +#include "rclcpp/rclcpp.hpp" + +#include +#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 +#include +#include +#include + +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 prevClusterCenters; + + cv::Mat state(stateDim, 1, CV_32F); + cv::Mat_ measurement(2, 1); + + std::vector objID; // Output of the data association using KF + // measurement.setTo(Scalar(0)); + + bool firstFrame = true; + /* + cv::Mat state(stateDim, 1, CV_32F); + cv::Mat_ 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::SharedPtr sub; + rclcpp::Publisher::SharedPtr markerPub; + rclcpp::Publisher::SharedPtr objID_pub; + rclcpp::Publisher::SharedPtr pub_cluster0; + rclcpp::Publisher::SharedPtr pub_cluster1; + rclcpp::Publisher::SharedPtr pub_cluster2; + rclcpp::Publisher::SharedPtr pub_cluster3; + rclcpp::Publisher::SharedPtr pub_cluster4; + rclcpp::Publisher::SharedPtr pub_cluster5; + + std::vector prevClusterCenters; + + std::vector 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 findIndexOfMin(std::vector> distMat); + void KFT(const std_msgs::msg::Float32MultiArray ccs); + void publish_cloud(rclcpp::Publisher::SharedPtr pub, + pcl::PointCloud::Ptr cluster); + + void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input, + std::vector::Ptr, float>> &cluster_vec, + std::vector &clusterCentroids); + void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input); + }; + +} +#endif // CLOUD_CLUSTERING_HPP diff --git a/src/target_tracking/src/cloud_clustering.cpp b/src/target_tracking/src/cloud_clustering.cpp index 3ebce4a..04f8588 100644 --- a/src/target_tracking/src/cloud_clustering.cpp +++ b/src/target_tracking/src/cloud_clustering.cpp @@ -1,664 +1,625 @@ -#include -#include -#include -#include -#include -#include -#include +// Default C++ imports +#include +#include +#include #include #include -#include -#include -#include +#include +#include + +// Headerfile +#include "cloud_clustering.hpp" + +// Ros2 import #include + +#include +#include + +// PCL Types +#include +#include + +// Ros2 messages #include #include -#include +#include +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include #include #include #include #include #include -#include -#include -#include -#include -#include - -using namespace std; -using namespace cv; - -rclcpp::Publisher::SharedPtr objID_pub; - -// 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); - -rclcpp::Publisher::SharedPtr pub_cluster0; -rclcpp::Publisher::SharedPtr pub_cluster1; -rclcpp::Publisher::SharedPtr pub_cluster2; -rclcpp::Publisher::SharedPtr pub_cluster3; -rclcpp::Publisher::SharedPtr pub_cluster4; -rclcpp::Publisher::SharedPtr pub_cluster5; - -rclcpp::Publisher::SharedPtr markerPub; - -std::vector prevClusterCenters; - -cv::Mat state(stateDim, 1, CV_32F); -cv::Mat_ measurement(2, 1); - -std::vector objID; // Output of the data association using KF - // measurement.setTo(Scalar(0)); - -bool firstFrame = true; - -// calculate euclidean distance of two points -double euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2) { - return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + - (p1.z - p2.z) * (p1.z - p2.z) * z_dim_scale); -} -/* -//Count unique object IDs. just to make sure same ID has not been assigned to -two KF_Trackers. int countIDs(vector v) +namespace cloud_clustering { - transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = -distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log -n), worst case O(n^2) (usually implemented as quicksort. - // To guarantee worst case O(n log n) replace with make_heap, then -sort_heap. + static const rclcpp::Logger LOGGER = rclcpp::get_logger("cloud_clustering"); - // Unique will take a sorted range, and move things around to get duplicated - // items to the back and returns an iterator to the end of the unique -section of the range auto unique_end = unique(v.begin(), v.end()); // Again n -comparisons return distance(unique_end, v.begin()); // Constant time for random -access iterators (like vector's) -} -*/ + CloudClustering::CloudClustering( + const rclcpp::NodeOptions &options) : CloudClustering("", options) + { + } -/* + CloudClustering::CloudClustering( + const std::string &name_space, + const rclcpp::NodeOptions &options) : Node("CloudClustering", name_space, options) + { + // Topic parameters + this->declare_parameter("topic_in", "filtered_points"); + this->declare_parameter("frame_id", "velodyne"); -objID: vector containing the IDs of the clusters that should be associated with -each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. -*/ + // Cluster parameters + this->declare_parameter("z_dim_scale", 0); + this->declare_parameter("cluster_tolerance", 0.0f); + this->declare_parameter("min_cluster_size", 0); + this->declare_parameter("max_cluster_size", 0); -std::pair findIndexOfMin(std::vector> distMat) { - std::pair minIndex; - float minEl = std::numeric_limits::max(); - for (int i = 0; i < distMat.size(); i++) - for (int j = 0; j < distMat.at(0).size(); j++) { - if (distMat[i][j] < minEl) { - minEl = distMat[i][j]; - minIndex = std::make_pair(i, j); + this->get_parameter("topic_in", topic_in); + this->get_parameter("frame_id", frame_id); + this->get_parameter("z_dim_scale", z_dim_scale); + this->get_parameter("cluster_tolerance", cluster_tolerance); + this->get_parameter("min_cluster_size", min_cluster_size); + this->get_parameter("max_cluster_size", max_cluster_size); + + // Initialize publishers + objID_pub = this->create_publisher("object_ids", 10); + pub_cluster0 = this->create_publisher("cluster0", 10); + pub_cluster1 = this->create_publisher("cluster1", 10); + pub_cluster2 = this->create_publisher("cluster2", 10); + pub_cluster3 = this->create_publisher("cluster3", 10); + pub_cluster4 = this->create_publisher("cluster4", 10); + pub_cluster5 = this->create_publisher("cluster5", 10); + markerPub = this->create_publisher("markers", 10); + + RCLCPP_INFO(this->get_logger(), "About to setup callback"); + clock_ = this->get_clock(); + + // Create a ROS subscriber for the input point cloud + sub = this->create_subscription( + topic_in, + 10, // queue size + std::bind(&CloudClustering::cloud_cb, this, std::placeholders::_1)); + + std::cout << "Started clustering node with parameters:\n" + << "topic_in: " << topic_in << "\n" + << "frame_id: " << frame_id << "\n" + << "z_dim_scale: " << z_dim_scale << "\n" + << "cluster_tolerance: " << cluster_tolerance << "\n" + << "min_cluster_size: " << min_cluster_size << "\n" + << "max_cluster_size: " << max_cluster_size << "\n"; + } + + // calculate euclidean distance of two points + double CloudClustering::euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2) + { + return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + + (p1.z - p2.z) * (p1.z - p2.z) * z_dim_scale); + } + /* + //Count unique object IDs. just to make sure same ID has not been assigned to + two KF_Trackers. int countIDs(vector v) + { + transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = + distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log + n), worst case O(n^2) (usually implemented as quicksort. + // To guarantee worst case O(n log n) replace with make_heap, then + sort_heap. + + // Unique will take a sorted range, and move things around to get duplicated + // items to the back and returns an iterator to the end of the unique + section of the range auto unique_end = unique(v.begin(), v.end()); // Again n + comparisons return distance(unique_end, v.begin()); // Constant time for random + access iterators (like vector's) + } + */ + + /* + + objID: vector containing the IDs of the clusters that should be associated with + each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. + */ + + std::pair CloudClustering::findIndexOfMin(std::vector> distMat) + { + std::pair minIndex; + float minEl = std::numeric_limits::max(); + for (int i = 0; i < distMat.size(); i++) + for (int j = 0; j < distMat.at(0).size(); j++) + { + if (distMat[i][j] < minEl) + { + minEl = distMat[i][j]; + minIndex = std::make_pair(i, j); + } } - } - return minIndex; -} -void KFT(const std_msgs::msg::Float32MultiArray ccs) { - - // First predict, to update the internal statePre variable - - std::vector pred{KF0.predict(), KF1.predict(), KF2.predict(), - KF3.predict(), KF4.predict(), KF5.predict()}; - - // cv::Point predictPt(prediction.at(0),prediction.at(1)); - // cout<<"Prediction 1 - // ="<(0)<<","<(1)<<"\n"; - - // Get measurements - // Extract the position of the clusters forom the multiArray. To check if the - // data coming in, check the .z (every third) coordinate and that will be 0.0 - std::vector clusterCenters; // clusterCenters - - int i = 0; - for (std::vector::const_iterator it = ccs.data.begin(); - it != ccs.data.end(); it += 3) { - geometry_msgs::msg::Point pt; - pt.x = *it; - pt.y = *(it + 1); - pt.z = *(it + 2); - - clusterCenters.push_back(pt); + return minIndex; } - std::vector KFpredictions; - i = 0; - for (auto it = pred.begin(); it != pred.end(); it++) { - geometry_msgs::msg::Point pt; - pt.x = (*it).at(0); - pt.y = (*it).at(1); - pt.z = (*it).at(2); + void CloudClustering::KFT(const std_msgs::msg::Float32MultiArray ccs) + { - KFpredictions.push_back(pt); - } + // First predict, to update the internal statePre variable - // Find the cluster that is more probable to be belonging to a given KF. - objID.clear(); // Clear the objID vector - objID.resize(6); // Allocate default elements so that [i] doesnt segfault. - // Should be done better - // Copy clusterCentres for modifying it and preventing multiple assignments of - // the same ID - std::vector copyOfClusterCenters(clusterCenters); - std::vector> distMat; + std::vector pred{KF0.predict(), KF1.predict(), KF2.predict(), + KF3.predict(), KF4.predict(), KF5.predict()}; - for (int filterN = 0; filterN < 6; filterN++) { - std::vector distVec; - for (int n = 0; n < 6; n++) { - distVec.push_back( - euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n])); - } + // cv::Point predictPt(prediction.at(0),prediction.at(1)); + // cout<<"Prediction 1 + // ="<(0)<<","<(1)<<"\n"; - distMat.push_back(distVec); - /*// Based on distVec instead of distMat (global min). Has problems with the - person's leg going out of scope int - ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())); - //cout<<"finterlN="< clusterCenters; // clusterCenters - for (int clusterCount = 0; clusterCount < 6; clusterCount++) { - // 1. Find min(distMax)==> (i,j); - std::pair minIndex(findIndexOfMin(distMat)); - // 2. objID[i]=clusterCenters[j]; counter++ - objID[minIndex.first] = minIndex.second; - - // 3. distMat[i,:]=10000; distMat[:,j]=10000 - distMat[minIndex.first] = - std::vector(6, 10000.0); // Set the row to a high number. - for (int row = 0; row < distMat.size(); - row++) // set the column to a high number - { - distMat[row][minIndex.second] = 10000.0; - } - // 4. if(counter<6) got to 1. - //cout << "clusterCount=" << clusterCount << "\n"; - } - - // cout<<"Got object IDs"<<"\n"; - // countIDs(objID);// for verif/corner cases - - // display objIDs - /* DEBUG - cout<<"objID= "; - for(auto it=objID.begin();it!=objID.end();it++) - cout<<*it<<" ,"; - cout<<"\n"; - */ - - visualization_msgs::msg::MarkerArray clusterMarkers; - - for (int i = 0; i < 6; i++) { - visualization_msgs::msg::Marker m; - - m.id = i; - m.type = visualization_msgs::msg::Marker::CUBE; - m.header.frame_id = frame_id; - m.scale.x = 0.3; - m.scale.y = 0.3; - m.scale.z = 0.3; - m.action = visualization_msgs::msg::Marker::ADD; - m.color.a = 1.0; - m.color.r = i % 2 ? 1 : 0; - m.color.g = i % 3 ? 1 : 0; - m.color.b = i % 4 ? 1 : 0; - - // geometry_msgs::msg::Point clusterC(clusterCenters.at(objID[i])); - geometry_msgs::msg::Point clusterC(KFpredictions[i]); - m.pose.position.x = clusterC.x; - m.pose.position.y = clusterC.y; - m.pose.position.z = clusterC.z; - - clusterMarkers.markers.push_back(m); - } - - prevClusterCenters = clusterCenters; - - markerPub->publish(clusterMarkers); - - std_msgs::msg::Int32MultiArray obj_id; - for (auto it = objID.begin(); it != objID.end(); it++) - obj_id.data.push_back(*it); - // Publish the object IDs - objID_pub->publish(obj_id); - // convert clusterCenters from geometry_msgs::msg::Point to floats - std::vector> cc; - for (int i = 0; i < 6; i++) { - vector pt; - pt.push_back(clusterCenters[objID[i]].x); - pt.push_back(clusterCenters[objID[i]].y); - pt.push_back(clusterCenters[objID[i]].z); - - cc.push_back(pt); - } - // cout<<"cc[5][0]="<(0, 0) == 0.0f || meas0Mat.at(1, 0) == 0.0f)) - Mat estimated0 = KF0.correct(meas0Mat); - if (!(meas1[0] == 0.0f || meas1[1] == 0.0f)) - Mat estimated1 = KF1.correct(meas1Mat); - if (!(meas2[0] == 0.0f || meas2[1] == 0.0f)) - Mat estimated2 = KF2.correct(meas2Mat); - if (!(meas3[0] == 0.0f || meas3[1] == 0.0f)) - Mat estimated3 = KF3.correct(meas3Mat); - if (!(meas4[0] == 0.0f || meas4[1] == 0.0f)) - Mat estimated4 = KF4.correct(meas4Mat); - if (!(meas5[0] == 0.0f || meas5[1] == 0.0f)) - Mat estimated5 = KF5.correct(meas5Mat); - - // Publish the point clouds belonging to each clusters - - // cout<<"estimate="<(0)<<","<(1)<<"\n"; - // Point statePt(estimated.at(0),estimated.at(1)); - // cout<<"DONE KF_TRACKER\n"; -} -void publish_cloud( - rclcpp::Publisher::SharedPtr pub, - pcl::PointCloud::Ptr cluster, - rclcpp::Node::SharedPtr node) -{ - // Create a PointCloud2 message - auto clustermsg = std::make_shared(); - - // Convert PCL cloud to ROS 2 message - pcl::toROSMsg(*cluster, *clustermsg); - - // Set header info - clustermsg->header.frame_id = frame_id; - clustermsg->header.stamp = node->now(); - - // Publish the message - pub->publish(*clustermsg); -} - -void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input, - std::vector::Ptr - ,float>>& cluster_vec, - std::vector& clusterCentroids - ) { - pcl::PointCloud::Ptr input_a(new pcl::PointCloud); - pcl::fromROSMsg(*input, *input_a); - - // Apply filters - pcl::PointCloud::Ptr fov_filtered(new pcl::PointCloud); - fov_filtered->reserve(input_a->size()); - - for (const auto& point : input_a->points) { - if (point.x < 0 || point.x > 40) continue; - if (isnan(point.x) || isnan(point.y) || isnan(point.z)) continue; - if (point.z < -0.5 || point.z > 1.5) continue; - fov_filtered->points.push_back(point); // emplace_back also works - } - - if (fov_filtered->empty()) return; // nothing to cluster - - pcl::PointCloud::Ptr input_cloud = fov_filtered; - - // Setup KdTree - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(input_cloud); - - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(cluster_tolerance); - ec.setMinClusterSize(min_cluster_size); - ec.setMaxClusterSize(max_cluster_size); - ec.setSearchMethod(tree); - ec.setInputCloud(input_cloud); - /* Extract the clusters out of pc and save indices in cluster_indices.*/ - ec.extract(cluster_indices); - - std::vector::const_iterator it; - std::vector::const_iterator pit; - - for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { - - pcl::PointCloud::Ptr cloud_cluster( - new pcl::PointCloud); - float x = 0.0; - float y = 0.0; - int numPts = 0; - float intensity_sum = 0.0; - - for (pit = it->indices.begin(); pit != it->indices.end(); pit++) { - - cloud_cluster->points.push_back(input_cloud->points[*pit]); - x += input_cloud->points[*pit].x; - y += input_cloud->points[*pit].y; - numPts++; - - intensity_sum += input_cloud->points[*pit].intensity; - // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], - // origin); - // mindist_this_cluster = std::min(dist_this_point, - // mindist_this_cluster); - } - - if (numPts == 0) continue; - - pcl::PointXYZI centroid; - centroid.x = x / numPts; - centroid.y = y / numPts; - centroid.z = 0.0; - - cluster_vec.push_back(std::make_pair(cloud_cluster, intensity_sum / numPts)); - - // Get the centroid of the cluster - clusterCentroids.push_back(centroid); - } - - // Ensure at least 6 clusters exist to publish (later clusters may be empty) - while (cluster_vec.size() < 6) { - pcl::PointCloud::Ptr empty_cluster( - new pcl::PointCloud); - - pcl::PointXYZI pt; - pt.x = pt.y = pt.z = pt.intensity = 0.0f; - - empty_cluster->points.push_back(pt); - - pcl::PointXYZI centroid; - centroid.x = 0.0; - centroid.y = 0.0; - centroid.z = 0.0; - centroid.intensity = 0.0; - - cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f)); - - } - - while (clusterCentroids.size() < 6) { - pcl::PointXYZI centroid; - centroid.x = 0.0; - centroid.y = 0.0; - centroid.z = 0.0; - centroid.intensity = 0.0; - - clusterCentroids.push_back(centroid); - } -} - -void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input, rclcpp::Node::SharedPtr node) - -{ - // Vector of cluster pointclouds - std::vector::Ptr,float>> cluster_vec; - // Cluster centroids - std::vector clusterCentroids; - - if (firstFrame) { - // If this is the first frame, initialize kalman filters for the clustered objects - // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. - // Could be made generic by creating a Kalman Filter only when a new object - // is detected - - float dvx = 0.01f; // 1.0 - float dvy = 0.01f; // 1.0 - float dx = 1.0f; - float dy = 1.0f; - KF0.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, - dvx, 0, 0, 0, 0, dvy); - KF1.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, - dvx, 0, 0, 0, 0, dvy); - KF2.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, - dvx, 0, 0, 0, 0, dvy); - KF3.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, - dvx, 0, 0, 0, 0, dvy); - KF4.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, - dvx, 0, 0, 0, 0, dvy); - KF5.transitionMatrix = (Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, - dvx, 0, 0, 0, 0, dvy); - - cv::setIdentity(KF0.measurementMatrix); - cv::setIdentity(KF1.measurementMatrix); - cv::setIdentity(KF2.measurementMatrix); - cv::setIdentity(KF3.measurementMatrix); - cv::setIdentity(KF4.measurementMatrix); - cv::setIdentity(KF5.measurementMatrix); - // Process Noise Covariance Matrix Q - // [ Ex 0 0 0 0 0 ] - // [ 0 Ey 0 0 0 0 ] - // [ 0 0 Ev_x 0 0 0 ] - // [ 0 0 0 1 Ev_y 0 ] - //// [ 0 0 0 0 1 Ew ] - //// [ 0 0 0 0 0 Eh ] - float sigmaP = 0.01; - float sigmaQ = 0.1; - setIdentity(KF0.processNoiseCov, Scalar::all(sigmaP)); - setIdentity(KF1.processNoiseCov, Scalar::all(sigmaP)); - setIdentity(KF2.processNoiseCov, Scalar::all(sigmaP)); - setIdentity(KF3.processNoiseCov, Scalar::all(sigmaP)); - setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP)); - setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP)); - // Meas noise cov matrix R - cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1 - cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ)); - cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ)); - - // Process the point cloud - parse_cloud(input, cluster_vec, clusterCentroids); - - // Set initial state - KF0.statePre.at(0) = clusterCentroids.at(0).x; - KF0.statePre.at(1) = clusterCentroids.at(0).y; - KF0.statePre.at(2) = 0; // initial v_x - KF0.statePre.at(3) = 0; // initial v_y - - // Set initial state - KF1.statePre.at(0) = clusterCentroids.at(1).x; - KF1.statePre.at(1) = clusterCentroids.at(1).y; - KF1.statePre.at(2) = 0; // initial v_x - KF1.statePre.at(3) = 0; // initial v_y - - // Set initial state - KF2.statePre.at(0) = clusterCentroids.at(2).x; - KF2.statePre.at(1) = clusterCentroids.at(2).y; - KF2.statePre.at(2) = 0; // initial v_x - KF2.statePre.at(3) = 0; // initial v_y - - // Set initial state - KF3.statePre.at(0) = clusterCentroids.at(3).x; - KF3.statePre.at(1) = clusterCentroids.at(3).y; - KF3.statePre.at(2) = 0; // initial v_x - KF3.statePre.at(3) = 0; // initial v_y - - // Set initial state - KF4.statePre.at(0) = clusterCentroids.at(4).x; - KF4.statePre.at(1) = clusterCentroids.at(4).y; - KF4.statePre.at(2) = 0; // initial v_x - KF4.statePre.at(3) = 0; // initial v_y - - // Set initial state - KF5.statePre.at(0) = clusterCentroids.at(5).x; - KF5.statePre.at(1) = clusterCentroids.at(5).y; - KF5.statePre.at(2) = 0; // initial v_x - KF5.statePre.at(3) = 0; // initial v_y - - firstFrame = false; - - for (int i = 0; i < 6; i++) { - geometry_msgs::msg::Point pt; - pt.x = clusterCentroids.at(i).x; - pt.y = clusterCentroids.at(i).y; - prevClusterCenters.push_back(pt); - } - } - - else { - parse_cloud(input, cluster_vec, clusterCentroids); - - std_msgs::msg::Float32MultiArray cc; - for (int i = 0; i < 6; i++) { - cc.data.push_back(clusterCentroids.at(i).x); - cc.data.push_back(clusterCentroids.at(i).y); - cc.data.push_back(clusterCentroids.at(i).z); - } - - // cc_pos->publish(cc);// Publish cluster mid-points. - KFT(cc); int i = 0; + for (std::vector::const_iterator it = ccs.data.begin(); + it != ccs.data.end(); it += 3) + { + geometry_msgs::msg::Point pt; + pt.x = *it; + pt.y = *(it + 1); + pt.z = *(it + 2); - std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b) { - return cluster_vec[a].second < cluster_vec[b].second; - }); + clusterCenters.push_back(pt); + } - for (auto it = objID.begin(); it != objID.end(); it++) { - switch (i) { - case 0: { + std::vector KFpredictions; + i = 0; + for (auto it = pred.begin(); it != pred.end(); it++) + { + geometry_msgs::msg::Point pt; + pt.x = (*it).at(0); + pt.y = (*it).at(1); + pt.z = (*it).at(2); - publish_cloud(pub_cluster0, cluster_vec[*it].first, node); - i++; - break; - } - case 1: { - publish_cloud(pub_cluster1, cluster_vec[*it].first, node); - i++; - break; - } - case 2: { - publish_cloud(pub_cluster2, cluster_vec[*it].first, node); - i++; - break; - } - case 3: { - publish_cloud(pub_cluster3, cluster_vec[*it].first, node); - i++; - break; - } - case 4: { - publish_cloud(pub_cluster4, cluster_vec[*it].first, node); - i++; - break; + KFpredictions.push_back(pt); + } + + // Find the cluster that is more probable to be belonging to a given KF. + objID.clear(); // Clear the objID vector + objID.resize(6); // Allocate default elements so that [i] doesnt segfault. + // Should be done better + // Copy clusterCentres for modifying it and preventing multiple assignments of + // the same ID + std::vector copyOfClusterCenters(clusterCenters); + std::vector> distMat; + + for (int filterN = 0; filterN < 6; filterN++) + { + std::vector distVec; + for (int n = 0; n < 6; n++) + { + distVec.push_back( + euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n])); } - case 5: { - publish_cloud(pub_cluster5, cluster_vec[*it].first, node); - i++; - break; + distMat.push_back(distVec); + /*// Based on distVec instead of distMat (global min). Has problems with the + person's leg going out of scope int + ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())); + //cout<<"finterlN="< (i,j); + std::pair minIndex(findIndexOfMin(distMat)); + // 2. objID[i]=clusterCenters[j]; counter++ + objID[minIndex.first] = minIndex.second; + + // 3. distMat[i,:]=10000; distMat[:,j]=10000 + distMat[minIndex.first] = + std::vector(6, 10000.0); // Set the row to a high number. + for (int row = 0; row < distMat.size(); + row++) // set the column to a high number + { + distMat[row][minIndex.second] = 10000.0; } - default: - break; + // 4. if(counter<6) got to 1. + // cout << "clusterCount=" << clusterCount << "\n"; + } + + // cout<<"Got object IDs"<<"\n"; + // countIDs(objID);// for verif/corner cases + + // display objIDs + /* DEBUG + cout<<"objID= "; + for(auto it=objID.begin();it!=objID.end();it++) + cout<<*it<<" ,"; + cout<<"\n"; + */ + + visualization_msgs::msg::MarkerArray clusterMarkers; + + for (int i = 0; i < 6; i++) + { + visualization_msgs::msg::Marker m; + + m.id = i; + m.type = visualization_msgs::msg::Marker::CUBE; + m.header.frame_id = frame_id; + m.scale.x = 0.3; + m.scale.y = 0.3; + m.scale.z = 0.3; + m.action = visualization_msgs::msg::Marker::ADD; + m.color.a = 1.0; + m.color.r = i % 2 ? 1 : 0; + m.color.g = i % 3 ? 1 : 0; + m.color.b = i % 4 ? 1 : 0; + + // geometry_msgs::msg::Point clusterC(clusterCenters.at(objID[i])); + geometry_msgs::msg::Point clusterC(KFpredictions[i]); + m.pose.position.x = clusterC.x; + m.pose.position.y = clusterC.y; + m.pose.position.z = clusterC.z; + + clusterMarkers.markers.push_back(m); + } + + prevClusterCenters = clusterCenters; + + markerPub->publish(clusterMarkers); + + std_msgs::msg::Int32MultiArray obj_id; + for (auto it = objID.begin(); it != objID.end(); it++) + obj_id.data.push_back(*it); + // Publish the object IDs + objID_pub->publish(obj_id); + // convert clusterCenters from geometry_msgs::msg::Point to floats + std::vector> cc; + for (int i = 0; i < 6; i++) + { + std::vector pt; + pt.push_back(clusterCenters[objID[i]].x); + pt.push_back(clusterCenters[objID[i]].y); + pt.push_back(clusterCenters[objID[i]].z); + + cc.push_back(pt); + } + // cout<<"cc[5][0]="<(0, 0) == 0.0f || meas0Mat.at(1, 0) == 0.0f)) + cv::Mat estimated0 = KF0.correct(meas0Mat); + if (!(meas1[0] == 0.0f || meas1[1] == 0.0f)) + cv::Mat estimated1 = KF1.correct(meas1Mat); + if (!(meas2[0] == 0.0f || meas2[1] == 0.0f)) + cv::Mat estimated2 = KF2.correct(meas2Mat); + if (!(meas3[0] == 0.0f || meas3[1] == 0.0f)) + cv::Mat estimated3 = KF3.correct(meas3Mat); + if (!(meas4[0] == 0.0f || meas4[1] == 0.0f)) + cv::Mat estimated4 = KF4.correct(meas4Mat); + if (!(meas5[0] == 0.0f || meas5[1] == 0.0f)) + cv::Mat estimated5 = KF5.correct(meas5Mat); + + // Publish the point clouds belonging to each clusters + + // cout<<"estimate="<(0)<<","<(1)<<"\n"; + // Point statePt(estimated.at(0),estimated.at(1)); + // cout<<"DONE KF_TRACKER\n"; + } + void CloudClustering::publish_cloud( + rclcpp::Publisher::SharedPtr pub, + pcl::PointCloud::Ptr cluster) + { + // Create a PointCloud2 message + auto clustermsg = std::make_shared(); + + // Convert PCL cloud to ROS 2 message + pcl::toROSMsg(*cluster, *clustermsg); + + // Set header info + clustermsg->header.frame_id = frame_id; + clustermsg->header.stamp = clock_->now(); + + // Publish the message + pub->publish(*clustermsg); + } + + void CloudClustering::parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input, + std::vector::Ptr, float>> &cluster_vec, + std::vector &clusterCentroids) + { + + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*input, *input_cloud); + + // Setup KdTree + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(input_cloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(cluster_tolerance); + ec.setMinClusterSize(min_cluster_size); + ec.setMaxClusterSize(max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(input_cloud); + /* Extract the clusters out of pc and save indices in cluster_indices.*/ + ec.extract(cluster_indices); + + std::vector::const_iterator it; + std::vector::const_iterator pit; + + for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) + { + + pcl::PointCloud::Ptr cloud_cluster( + new pcl::PointCloud); + float x = 0.0; + float y = 0.0; + int numPts = 0; + float intensity_sum = 0.0; + + for (pit = it->indices.begin(); pit != it->indices.end(); pit++) + { + + cloud_cluster->points.push_back(input_cloud->points[*pit]); + x += input_cloud->points[*pit].x; + y += input_cloud->points[*pit].y; + numPts++; + + intensity_sum += input_cloud->points[*pit].intensity; + // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], + // origin); + // mindist_this_cluster = std::min(dist_this_point, + // mindist_this_cluster); + } + + if (numPts == 0) + continue; + + pcl::PointXYZI centroid; + centroid.x = x / numPts; + centroid.y = y / numPts; + centroid.z = 0.0; + + cluster_vec.push_back(std::make_pair(cloud_cluster, intensity_sum / numPts)); + + // Get the centroid of the cluster + clusterCentroids.push_back(centroid); + } + + // Ensure at least 6 clusters exist to publish (later clusters may be empty) + while (cluster_vec.size() < 6) + { + pcl::PointCloud::Ptr empty_cluster( + new pcl::PointCloud); + + pcl::PointXYZI pt; + pt.x = pt.y = pt.z = pt.intensity = 0.0f; + + empty_cluster->points.push_back(pt); + + pcl::PointXYZI centroid; + centroid.x = 0.0; + centroid.y = 0.0; + centroid.z = 0.0; + centroid.intensity = 0.0; + + cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f)); + } + + while (clusterCentroids.size() < 6) + { + pcl::PointXYZI centroid; + centroid.x = 0.0; + centroid.y = 0.0; + centroid.z = 0.0; + centroid.intensity = 0.0; + + clusterCentroids.push_back(centroid); + } + } + + void CloudClustering::cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input) + + { + // Vector of cluster pointclouds + std::vector::Ptr, float>> cluster_vec; + // Cluster centroids + std::vector clusterCentroids; + + if (firstFrame) + { + // If this is the first frame, initialize kalman filters for the clustered objects + // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. + // Could be made generic by creating a Kalman Filter only when a new object + // is detected + + float dvx = 0.01f; // 1.0 + float dvy = 0.01f; // 1.0 + float dx = 1.0f; + float dy = 1.0f; + KF0.transitionMatrix = (cv::Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF1.transitionMatrix = (cv::Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF2.transitionMatrix = (cv::Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF3.transitionMatrix = (cv::Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF4.transitionMatrix = (cv::Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + KF5.transitionMatrix = (cv::Mat_(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0, + dvx, 0, 0, 0, 0, dvy); + + cv::setIdentity(KF0.measurementMatrix); + cv::setIdentity(KF1.measurementMatrix); + cv::setIdentity(KF2.measurementMatrix); + cv::setIdentity(KF3.measurementMatrix); + cv::setIdentity(KF4.measurementMatrix); + cv::setIdentity(KF5.measurementMatrix); + // Process Noise Covariance Matrix Q + // [ Ex 0 0 0 0 0 ] + // [ 0 Ey 0 0 0 0 ] + // [ 0 0 Ev_x 0 0 0 ] + // [ 0 0 0 1 Ev_y 0 ] + //// [ 0 0 0 0 1 Ew ] + //// [ 0 0 0 0 0 Eh ] + float sigmaP = 0.01; + float sigmaQ = 0.1; + setIdentity(KF0.processNoiseCov, cv::Scalar::all(sigmaP)); + setIdentity(KF1.processNoiseCov, cv::Scalar::all(sigmaP)); + setIdentity(KF2.processNoiseCov, cv::Scalar::all(sigmaP)); + setIdentity(KF3.processNoiseCov, cv::Scalar::all(sigmaP)); + setIdentity(KF4.processNoiseCov, cv::Scalar::all(sigmaP)); + setIdentity(KF5.processNoiseCov, cv::Scalar::all(sigmaP)); + // Meas noise cov matrix R + cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1 + cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ)); + cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ)); + + // Process the point cloud + parse_cloud(input, cluster_vec, clusterCentroids); + + // Set initial state + KF0.statePre.at(0) = clusterCentroids.at(0).x; + KF0.statePre.at(1) = clusterCentroids.at(0).y; + KF0.statePre.at(2) = 0; // initial v_x + KF0.statePre.at(3) = 0; // initial v_y + + // Set initial state + KF1.statePre.at(0) = clusterCentroids.at(1).x; + KF1.statePre.at(1) = clusterCentroids.at(1).y; + KF1.statePre.at(2) = 0; // initial v_x + KF1.statePre.at(3) = 0; // initial v_y + + // Set initial state + KF2.statePre.at(0) = clusterCentroids.at(2).x; + KF2.statePre.at(1) = clusterCentroids.at(2).y; + KF2.statePre.at(2) = 0; // initial v_x + KF2.statePre.at(3) = 0; // initial v_y + + // Set initial state + KF3.statePre.at(0) = clusterCentroids.at(3).x; + KF3.statePre.at(1) = clusterCentroids.at(3).y; + KF3.statePre.at(2) = 0; // initial v_x + KF3.statePre.at(3) = 0; // initial v_y + + // Set initial state + KF4.statePre.at(0) = clusterCentroids.at(4).x; + KF4.statePre.at(1) = clusterCentroids.at(4).y; + KF4.statePre.at(2) = 0; // initial v_x + KF4.statePre.at(3) = 0; // initial v_y + + // Set initial state + KF5.statePre.at(0) = clusterCentroids.at(5).x; + KF5.statePre.at(1) = clusterCentroids.at(5).y; + KF5.statePre.at(2) = 0; // initial v_x + KF5.statePre.at(3) = 0; // initial v_y + + firstFrame = false; + + for (int i = 0; i < 6; i++) + { + geometry_msgs::msg::Point pt; + pt.x = clusterCentroids.at(i).x; + pt.y = clusterCentroids.at(i).y; + prevClusterCenters.push_back(pt); + } + } + + else + { + parse_cloud(input, cluster_vec, clusterCentroids); + + std_msgs::msg::Float32MultiArray cc; + for (int i = 0; i < 6; i++) + { + cc.data.push_back(clusterCentroids.at(i).x); + cc.data.push_back(clusterCentroids.at(i).y); + cc.data.push_back(clusterCentroids.at(i).z); + } + + // cc_pos->publish(cc);// Publish cluster mid-points. + KFT(cc); + int i = 0; + + std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b) + { return cluster_vec[a].second < cluster_vec[b].second; }); + + for (auto it = objID.begin(); it != objID.end(); it++) + { + switch (i) + { + case 0: + { + publish_cloud(pub_cluster0, cluster_vec[*it].first); + i++; + break; + } + case 1: + { + publish_cloud(pub_cluster1, cluster_vec[*it].first); + i++; + break; + } + case 2: + { + publish_cloud(pub_cluster2, cluster_vec[*it].first); + i++; + break; + } + case 3: + { + publish_cloud(pub_cluster3, cluster_vec[*it].first); + i++; + break; + } + case 4: + { + publish_cloud(pub_cluster4, cluster_vec[*it].first); + i++; + break; + } + + case 5: + { + publish_cloud(pub_cluster5, cluster_vec[*it].first); + i++; + break; + } + default: + break; + } } } } } - -int main(int argc, char **argv) { - // ROS init - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("cloud_clustering"); - - //Topic parameters - node->declare_parameter("topic_in", "!defaultTopic!"); - node->declare_parameter("frame_id", "!defaultFrameId!"); - - //Cluster parameters - node->declare_parameter("z_dim_scale", 0); - node->declare_parameter("cluster_tolerance", 0.0f); - node->declare_parameter("min_cluster_size", 0); - node->declare_parameter("max_cluster_size", 0); - - node->get_parameter("topic_in", topic_in); - node->get_parameter("frame_id", frame_id); - node->get_parameter("z_dim_scale", z_dim_scale); - node->get_parameter("cluster_tolerance", cluster_tolerance); - node->get_parameter("min_cluster_size", min_cluster_size); - node->get_parameter("max_cluster_size", max_cluster_size); - - // Initialize publishers - objID_pub = node->create_publisher("object_ids", 10); - pub_cluster0 = node->create_publisher("cluster0", 10); - pub_cluster1 = node->create_publisher("cluster1", 10); - pub_cluster2 = node->create_publisher("cluster2", 10); - pub_cluster3 = node->create_publisher("cluster3", 10); - pub_cluster4 = node->create_publisher("cluster4", 10); - pub_cluster5 = node->create_publisher("cluster5", 10); - markerPub = node->create_publisher("markers", 10); - - // Publishers to publish the state of the objects (pos and vel) - // objState1=nh.advertise ("obj_1",1); - - cout << "About to setup callback\n"; - - // Create a ROS subscriber for the input point cloud - auto sub = node->create_subscription( - topic_in, - 10, // queue size - [node](sensor_msgs::msg::PointCloud2::SharedPtr msg) { - cloud_cb(msg, node); - }); - - // Publishers - objID_pub = node->create_publisher("obj_id", 10); - markerPub = node->create_publisher("viz", 10); - - cout << "Started clustering node with parameters:\n" - << "topic_in: " << topic_in << "\n" - << "frame_id: " << frame_id << "\n" - << "z_dim_scale: " << z_dim_scale << "\n" - << "cluster_tolerance: " << cluster_tolerance << "\n" - << "min_cluster_size: " << min_cluster_size << "\n" - << "max_cluster_size: " << max_cluster_size << "\n"; - - // Example if you still need cluster centers publisher: - // cc_pos = node->create_publisher("ccs", 10); - - /* Point cloud clustering - */ - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/target_tracking/src/cloud_clustering_node.cpp b/src/target_tracking/src/cloud_clustering_node.cpp new file mode 100644 index 0000000..2a3ff9e --- /dev/null +++ b/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(options)); + + rclcpp::shutdown(); + return 0; +}