this shit ON FIRE
This commit is contained in:
69
src/target_tracking/CMakeLists.txt
Normal file
69
src/target_tracking/CMakeLists.txt
Normal file
@@ -0,0 +1,69 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(target_tracking)
|
||||
|
||||
# Required packages
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(PCL REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_sensor_msgs REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
|
||||
|
||||
# Include paths
|
||||
include_directories(
|
||||
include
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# Node for preprocessing
|
||||
add_executable(cloud_preprocessing_node
|
||||
src/cloud_preprocessing.cpp
|
||||
)
|
||||
|
||||
# Dependencies for preprocessing
|
||||
ament_target_dependencies(cloud_preprocessing_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
pcl_conversions
|
||||
tf2
|
||||
tf2_ros
|
||||
tf2_sensor_msgs
|
||||
)
|
||||
|
||||
# Node for clustering
|
||||
add_executable(cloud_clustering_node
|
||||
src/cloud_clustering.cpp
|
||||
)
|
||||
|
||||
# Dependencies for clustering
|
||||
ament_target_dependencies(cloud_clustering_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
visualization_msgs
|
||||
PCL
|
||||
pcl_conversions
|
||||
OpenCV
|
||||
)
|
||||
|
||||
#target_link_libraries(target_tracking_node ${PCL_LIBRARIES})
|
||||
|
||||
# Install target
|
||||
install(TARGETS
|
||||
cloud_preprocessing_node
|
||||
cloud_clustering_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install launchfile
|
||||
install(
|
||||
DIRECTORY launch/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
|
||||
ament_package()
|
||||
3
src/target_tracking/README.md
Normal file
3
src/target_tracking/README.md
Normal file
@@ -0,0 +1,3 @@
|
||||
# Target tracking
|
||||
|
||||
Ros2 implementation of a filtering node in combination with a clustering node to find a target with lidar data
|
||||
69
src/target_tracking/launch/target_tracking_launch.py
Normal file
69
src/target_tracking/launch/target_tracking_launch.py
Normal file
@@ -0,0 +1,69 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
import math
|
||||
|
||||
################### user configure parameters for ros2 start ###################
|
||||
# Topics/Frames
|
||||
frame_id = 'velodyne'
|
||||
topic_preprocessing_in = 'velodyne_points'
|
||||
topic_preprocessing_out = 'filtered_points'
|
||||
|
||||
# Preprocessing
|
||||
x_min = 0.0
|
||||
x_max = 20.0
|
||||
z_min = -0.5
|
||||
z_max = 1.5
|
||||
tan_h_fov = math.pi / 4 # ±45°
|
||||
tan_v_fov = math.pi / 6 # ±30°
|
||||
|
||||
# Clustering
|
||||
z_dim_scale = 0.2
|
||||
cluster_tolerance = 0.2
|
||||
min_cluster_size = 10
|
||||
max_cluster_size = 1000
|
||||
|
||||
################### user configure parameters for ros2 end #####################
|
||||
|
||||
cloud_preprocessing_params = [
|
||||
{"topic_in": topic_preprocessing_in},
|
||||
{"topic_out": topic_preprocessing_out},
|
||||
{"x_min": x_min},
|
||||
{"x_max": x_max},
|
||||
{"z_min": z_min},
|
||||
{"z_max": z_max},
|
||||
{"tan_h_fov": tan_h_fov},
|
||||
{"tan_v_fov": tan_v_fov}
|
||||
]
|
||||
|
||||
cloud_clustering_params = [
|
||||
{"topic_in": topic_preprocessing_out},
|
||||
{"frame_id": frame_id},
|
||||
{"z_dim_scale": z_dim_scale},
|
||||
{"cluster_tolerance": cluster_tolerance},
|
||||
{"min_cluster_size": min_cluster_size},
|
||||
{"max_cluster_size": max_cluster_size}
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='target_tracking',
|
||||
executable='cloud_preprocessing_node',
|
||||
name='cloud_preprocessing',
|
||||
parameters=cloud_preprocessing_params,
|
||||
output={
|
||||
'stdout': 'screen',
|
||||
'stderr': 'screen',
|
||||
}
|
||||
),
|
||||
Node(
|
||||
package='target_tracking',
|
||||
executable='cloud_clustering_node',
|
||||
name='cloud_clustering',
|
||||
parameters=cloud_clustering_params,
|
||||
output={
|
||||
'stdout': 'screen',
|
||||
'stderr': 'screen',
|
||||
}
|
||||
)
|
||||
])
|
||||
27
src/target_tracking/package.xml
Normal file
27
src/target_tracking/package.xml
Normal file
@@ -0,0 +1,27 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>target_tracking</name>
|
||||
<version>1.0.1</version>
|
||||
<description>Studienprojekt</description>
|
||||
<maintainer email="liam20030625@gmail.com">Liam</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<!-- Dependencies for voxel -->
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>PCL</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>OpenCV</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
664
src/target_tracking/src/cloud_clustering.cpp
Normal file
664
src/target_tracking/src/cloud_clustering.cpp
Normal file
@@ -0,0 +1,664 @@
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <algorithm>
|
||||
#include <fstream>
|
||||
#include <geometry_msgs/msg/point.hpp>
|
||||
#include <iostream>
|
||||
#include <iterator>
|
||||
#include <opencv2/video/video.hpp>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/float32_multi_array.hpp>
|
||||
#include <std_msgs/msg/int32_multi_array.hpp>
|
||||
#include <string.h>
|
||||
|
||||
#include <pcl/common/centroid.h>
|
||||
#include <pcl/common/geometry.h>
|
||||
#include <pcl/features/normal_3d.h>
|
||||
#include <pcl/filters/extract_indices.h>
|
||||
#include <pcl/filters/voxel_grid.h>
|
||||
#include <pcl/kdtree/kdtree.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/sample_consensus/method_types.h>
|
||||
#include <pcl/sample_consensus/model_types.h>
|
||||
#include <pcl/segmentation/extract_clusters.h>
|
||||
#include <pcl/segmentation/sac_segmentation.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
#include <limits>
|
||||
#include <utility>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::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<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;
|
||||
|
||||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markerPub;
|
||||
|
||||
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;
|
||||
|
||||
// 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<int> 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<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat) {
|
||||
std::pair<int, int> minIndex;
|
||||
float minEl = std::numeric_limits<float>::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<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
|
||||
KF3.predict(), KF4.predict(), KF5.predict()};
|
||||
|
||||
// cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
|
||||
// cout<<"Prediction 1
|
||||
// ="<<prediction.at<float>(0)<<","<<prediction.at<float>(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<geometry_msgs::msg::Point> clusterCenters; // clusterCenters
|
||||
|
||||
int i = 0;
|
||||
for (std::vector<float>::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);
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> KFpredictions;
|
||||
i = 0;
|
||||
for (auto it = pred.begin(); it != pred.end(); it++) {
|
||||
geometry_msgs::msg::Point pt;
|
||||
pt.x = (*it).at<float>(0);
|
||||
pt.y = (*it).at<float>(1);
|
||||
pt.z = (*it).at<float>(2);
|
||||
|
||||
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<geometry_msgs::msg::Point> copyOfClusterCenters(clusterCenters);
|
||||
std::vector<std::vector<float>> distMat;
|
||||
|
||||
for (int filterN = 0; filterN < 6; filterN++) {
|
||||
std::vector<float> distVec;
|
||||
for (int n = 0; n < 6; n++) {
|
||||
distVec.push_back(
|
||||
euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[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="<<filterN<<" minID="<<ID
|
||||
objID.push_back(ID);
|
||||
// Prevent assignment of the same object ID to multiple clusters
|
||||
copyOfClusterCenters[ID].x=100000;// A large value so that this center is
|
||||
not assigned to another cluster copyOfClusterCenters[ID].y=10000;
|
||||
copyOfClusterCenters[ID].z=10000;
|
||||
*/
|
||||
//cout << "filterN=" << filterN << "\n";
|
||||
}
|
||||
|
||||
for (int clusterCount = 0; clusterCount < 6; clusterCount++) {
|
||||
// 1. Find min(distMax)==> (i,j);
|
||||
std::pair<int, int> 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<float>(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<std::vector<float>> cc;
|
||||
for (int i = 0; i < 6; i++) {
|
||||
vector<float> 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]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
|
||||
float meas0[2] = {cc[0].at(0), cc[0].at(1)};
|
||||
float meas1[2] = {cc[1].at(0), cc[1].at(1)};
|
||||
float meas2[2] = {cc[2].at(0), cc[2].at(1)};
|
||||
float meas3[2] = {cc[3].at(0), cc[3].at(1)};
|
||||
float meas4[2] = {cc[4].at(0), cc[4].at(1)};
|
||||
float meas5[2] = {cc[5].at(0), cc[5].at(1)};
|
||||
|
||||
// The update phase
|
||||
cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
|
||||
cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
|
||||
cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
|
||||
cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
|
||||
cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
|
||||
cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);
|
||||
|
||||
// cout<<"meas0Mat"<<meas0Mat<<"\n";
|
||||
if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(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="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
|
||||
// Point statePt(estimated.at<float>(0),estimated.at<float>(1));
|
||||
// cout<<"DONE KF_TRACKER\n";
|
||||
}
|
||||
void publish_cloud(
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster,
|
||||
rclcpp::Node::SharedPtr node)
|
||||
{
|
||||
// Create a PointCloud2 message
|
||||
auto clustermsg = std::make_shared<sensor_msgs::msg::PointCloud2>();
|
||||
|
||||
// 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<std::pair<
|
||||
pcl::PointCloud<
|
||||
pcl::PointXYZI
|
||||
>::Ptr
|
||||
,float>>& cluster_vec,
|
||||
std::vector<pcl::PointXYZI>& clusterCentroids
|
||||
) {
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr input_a(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
pcl::fromROSMsg(*input, *input_a);
|
||||
|
||||
// Apply filters
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr fov_filtered(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
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<pcl::PointXYZI>::Ptr input_cloud = fov_filtered;
|
||||
|
||||
// Setup KdTree
|
||||
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
|
||||
tree->setInputCloud(input_cloud);
|
||||
|
||||
std::vector<pcl::PointIndices> cluster_indices;
|
||||
pcl::EuclideanClusterExtraction<pcl::PointXYZI> 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<pcl::PointIndices>::const_iterator it;
|
||||
std::vector<int>::const_iterator pit;
|
||||
|
||||
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(
|
||||
new pcl::PointCloud<pcl::PointXYZI>);
|
||||
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<pcl::PointXYZI>::Ptr empty_cluster(
|
||||
new pcl::PointCloud<pcl::PointXYZI>);
|
||||
|
||||
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<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr,float>> cluster_vec;
|
||||
// Cluster centroids
|
||||
std::vector<pcl::PointXYZI> 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_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
|
||||
dvx, 0, 0, 0, 0, dvy);
|
||||
KF1.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
|
||||
dvx, 0, 0, 0, 0, dvy);
|
||||
KF2.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
|
||||
dvx, 0, 0, 0, 0, dvy);
|
||||
KF3.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
|
||||
dvx, 0, 0, 0, 0, dvy);
|
||||
KF4.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
|
||||
dvx, 0, 0, 0, 0, dvy);
|
||||
KF5.transitionMatrix = (Mat_<float>(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<float>(0) = clusterCentroids.at(0).x;
|
||||
KF0.statePre.at<float>(1) = clusterCentroids.at(0).y;
|
||||
KF0.statePre.at<float>(2) = 0; // initial v_x
|
||||
KF0.statePre.at<float>(3) = 0; // initial v_y
|
||||
|
||||
// Set initial state
|
||||
KF1.statePre.at<float>(0) = clusterCentroids.at(1).x;
|
||||
KF1.statePre.at<float>(1) = clusterCentroids.at(1).y;
|
||||
KF1.statePre.at<float>(2) = 0; // initial v_x
|
||||
KF1.statePre.at<float>(3) = 0; // initial v_y
|
||||
|
||||
// Set initial state
|
||||
KF2.statePre.at<float>(0) = clusterCentroids.at(2).x;
|
||||
KF2.statePre.at<float>(1) = clusterCentroids.at(2).y;
|
||||
KF2.statePre.at<float>(2) = 0; // initial v_x
|
||||
KF2.statePre.at<float>(3) = 0; // initial v_y
|
||||
|
||||
// Set initial state
|
||||
KF3.statePre.at<float>(0) = clusterCentroids.at(3).x;
|
||||
KF3.statePre.at<float>(1) = clusterCentroids.at(3).y;
|
||||
KF3.statePre.at<float>(2) = 0; // initial v_x
|
||||
KF3.statePre.at<float>(3) = 0; // initial v_y
|
||||
|
||||
// Set initial state
|
||||
KF4.statePre.at<float>(0) = clusterCentroids.at(4).x;
|
||||
KF4.statePre.at<float>(1) = clusterCentroids.at(4).y;
|
||||
KF4.statePre.at<float>(2) = 0; // initial v_x
|
||||
KF4.statePre.at<float>(3) = 0; // initial v_y
|
||||
|
||||
// Set initial state
|
||||
KF5.statePre.at<float>(0) = clusterCentroids.at(5).x;
|
||||
KF5.statePre.at<float>(1) = clusterCentroids.at(5).y;
|
||||
KF5.statePre.at<float>(2) = 0; // initial v_x
|
||||
KF5.statePre.at<float>(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, 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;
|
||||
}
|
||||
|
||||
case 5: {
|
||||
publish_cloud(pub_cluster5, cluster_vec[*it].first, node);
|
||||
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<std::string>("topic_in", "!defaultTopic!");
|
||||
node->declare_parameter<std::string>("frame_id", "!defaultFrameId!");
|
||||
|
||||
//Cluster parameters
|
||||
node->declare_parameter<float>("z_dim_scale", 0);
|
||||
node->declare_parameter<float>("cluster_tolerance", 0.0f);
|
||||
node->declare_parameter<int>("min_cluster_size", 0);
|
||||
node->declare_parameter<int>("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<std_msgs::msg::Int32MultiArray>("object_ids", 10);
|
||||
pub_cluster0 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster0", 10);
|
||||
pub_cluster1 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster1", 10);
|
||||
pub_cluster2 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster2", 10);
|
||||
pub_cluster3 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster3", 10);
|
||||
pub_cluster4 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster4", 10);
|
||||
pub_cluster5 = node->create_publisher<sensor_msgs::msg::PointCloud2>("cluster5", 10);
|
||||
markerPub = node->create_publisher<visualization_msgs::msg::MarkerArray>("markers", 10);
|
||||
|
||||
// Publishers to publish the state of the objects (pos and vel)
|
||||
// objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
|
||||
|
||||
cout << "About to setup callback\n";
|
||||
|
||||
// Create a ROS subscriber for the input point cloud
|
||||
auto sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
topic_in,
|
||||
10, // queue size
|
||||
[node](sensor_msgs::msg::PointCloud2::SharedPtr msg) {
|
||||
cloud_cb(msg, node);
|
||||
});
|
||||
|
||||
// Publishers
|
||||
objID_pub = node->create_publisher<std_msgs::msg::Int32MultiArray>("obj_id", 10);
|
||||
markerPub = node->create_publisher<visualization_msgs::msg::MarkerArray>("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<std_msgs::msg::Float32MultiArray>("ccs", 10);
|
||||
|
||||
/* Point cloud clustering
|
||||
*/
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
166
src/target_tracking/src/cloud_preprocessing.cpp
Normal file
166
src/target_tracking/src/cloud_preprocessing.cpp
Normal file
@@ -0,0 +1,166 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
|
||||
#include <cmath>
|
||||
|
||||
#include <random>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/filters/voxel_grid.h>
|
||||
#include <pcl/filters/extract_indices.h>
|
||||
#include <pcl/segmentation/sac_segmentation.h>
|
||||
#include <pcl/sample_consensus/method_types.h>
|
||||
#include <pcl/sample_consensus/model_types.h>
|
||||
#include <pcl/ModelCoefficients.h>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
|
||||
class CloudFilterNode : public rclcpp::Node
|
||||
{
|
||||
//Topic parameters
|
||||
std::string topic_in;
|
||||
std::string topic_out;
|
||||
|
||||
// Filter parameters
|
||||
float x_min;
|
||||
float x_max;
|
||||
float z_min;
|
||||
float z_max;
|
||||
float tan_h_fov;
|
||||
float tan_v_fov;
|
||||
|
||||
public:
|
||||
CloudFilterNode()
|
||||
: Node("cloud_preprocessing"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
|
||||
{
|
||||
|
||||
//Topic parameters
|
||||
this->declare_parameter<std::string>("topic_in", "test");
|
||||
this->declare_parameter<std::string>("topic_out", "test2");
|
||||
|
||||
//Filter parameters
|
||||
this->declare_parameter<float>("x_min", 0.0f);
|
||||
this->declare_parameter<float>("x_max", 0.0f);
|
||||
this->declare_parameter<float>("z_min", 0.0f);
|
||||
this->declare_parameter<float>("z_max", 0.0f);
|
||||
this->declare_parameter<float>("tan_h_fov", 0.0f); // ±45°
|
||||
this->declare_parameter<float>("tan_v_fov", 0.0f); // ±30°
|
||||
|
||||
this->get_parameter("topic_in", topic_in);
|
||||
this->get_parameter("topic_out", topic_out);
|
||||
|
||||
this->get_parameter("x_min", x_min);
|
||||
this->get_parameter("x_max", x_max);
|
||||
this->get_parameter("z_min", z_min);
|
||||
this->get_parameter("z_max", z_max);
|
||||
this->get_parameter("tan_h_fov", tan_h_fov);
|
||||
this->get_parameter("tan_v_fov", tan_v_fov);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Starting filter node with parameters:\n"
|
||||
"topic_in: %s\n"
|
||||
"topic_out: %s\n"
|
||||
"x_min: %f\n"
|
||||
"x_max: %f\n"
|
||||
"z_min: %f\n"
|
||||
"z_max: %f\n"
|
||||
"tan_h_fov: %f\n"
|
||||
"tan_v_fov: %f"
|
||||
, topic_in.c_str(), topic_out.c_str(), x_min, x_max, z_min, z_max, tan_h_fov, tan_v_fov
|
||||
);
|
||||
|
||||
subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
topic_in, rclcpp::SensorDataQoS(),
|
||||
std::bind(&CloudFilterNode::cloud_callback, this, std::placeholders::_1));
|
||||
|
||||
filtered_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(topic_out, 10);
|
||||
}
|
||||
|
||||
private:
|
||||
void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
|
||||
{
|
||||
// Convert to PCL
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr input(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
pcl::fromROSMsg(*msg, *input);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr fov_filtered(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
fov_filtered->reserve(input->size());
|
||||
|
||||
for (const auto& point : input->points)
|
||||
{
|
||||
if (point.x < x_min || point.x > x_max) continue;
|
||||
if (isnan(point.x) || isnan(point.y) || isnan(point.z)) continue;
|
||||
|
||||
float inv_x = 1.0f / point.x;
|
||||
if (std::abs(point.y * inv_x) > tan_h_fov) continue;
|
||||
if (std::abs(point.z * inv_x) > tan_v_fov) continue;
|
||||
if (point.z < z_min || point.z > z_max) continue;
|
||||
fov_filtered->emplace_back(point);
|
||||
}
|
||||
|
||||
// Apply voxel grid filter
|
||||
pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
|
||||
voxel_filter.setInputCloud(fov_filtered);
|
||||
voxel_filter.setLeafSize(0.03f, 0.03f, 0.03f); // Adjust as needed
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_filtered(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
voxel_filter.filter(*voxel_filtered);
|
||||
|
||||
//Apply ransac
|
||||
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
|
||||
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
|
||||
// Create the segmentation object
|
||||
pcl::SACSegmentation<pcl::PointXYZI> seg;
|
||||
// Optional
|
||||
seg.setOptimizeCoefficients(true);
|
||||
// Mandatory
|
||||
seg.setModelType(pcl::SACMODEL_PLANE);
|
||||
seg.setMethodType(pcl::SAC_RANSAC);
|
||||
seg.setDistanceThreshold(0.01);
|
||||
|
||||
seg.setInputCloud(voxel_filtered);
|
||||
seg.segment (*inliers, *coefficients);
|
||||
|
||||
if (inliers->indices.size () == 0)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Oh oh. No inliers");
|
||||
return;
|
||||
}
|
||||
|
||||
//Select remaining points
|
||||
pcl::ExtractIndices<pcl::PointXYZI> extract;
|
||||
extract.setInputCloud(voxel_filtered);
|
||||
extract.setIndices(inliers);
|
||||
extract.setNegative(true);
|
||||
extract.filter(*voxel_filtered);
|
||||
|
||||
// Convert and publish
|
||||
sensor_msgs::msg::PointCloud2 out_msg;
|
||||
pcl::toROSMsg(*voxel_filtered, out_msg);
|
||||
out_msg.header.stamp = this->get_clock()->now();
|
||||
out_msg.header.frame_id = msg->header.frame_id;
|
||||
filtered_publisher_->publish(out_msg);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Filtered %zu -> %zu points", input->points.size(), voxel_filtered->points.size());
|
||||
}
|
||||
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_publisher_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr original_publisher_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
srand((unsigned int) time (NULL));
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<CloudFilterNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user