andrew/pcl_tutorial
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

colored clusters

Browse Source
This commit is contained in:
caroline 2022-05-02 14:07:50 +02:00
parent 1a8ccab6da
commit 8f4ec0bf9f
2 changed files with 57 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cfg/config.cfg
View File

@ -15,7 +15,7 @@ gen.add("zbpos", double_t, 0, "Max bound of Z - Axis cropping", 5, -5, 5)
gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping", 0, -5, 5)
gen.add("rotx", double_t, 0, "Rotation of cloud around X - Axis", 0, 0, 180)
gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 45, 0, 180)
gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 0, 0, 180)
gen.add("rotz", double_t, 0, "Rotation of cloud around Z - Axis", 0, 0, 180)
gen.add("movex", double_t, 0, "Move along X - Axis", 0, -10, 10)

107
src/initial_processing.cpp
View File

@ -40,6 +40,7 @@ ros::Publisher pub3;
ros::Publisher marker_pub;
ros::Publisher marker_pub2;
ros::Publisher visual;
ros::Publisher steer;
float xbpos = 5.0;
float xbneg = 0.0;
@ -157,7 +158,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
line_list.scale.x = 0.05;
line_list.color.g = 1.0;
line_list.color.a = 1.0;
line_list.header.frame_id = "zed_base_link";
line_list.header.frame_id = "base_link";
line_list.action = visualization_msgs::Marker::ADD;
line_list.pose.orientation.w = 1.0;
geometry_msgs::Point p1; geometry_msgs::Point p2; geometry_msgs::Point p3; geometry_msgs::Point p4;
@ -246,25 +247,13 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices); // indices are saved in cluster_indices
// seperating each cluster out of vector<PointIndices>
// => iterate through cluster_indices, create new PointCloud for each entry and write all points of respective cluster into that PointCloud
//int j = 0;
// std::vector<std::vector<pcl::PointCloud<PointT>>> cluster_vector(2, std::vector<pcl::PointCloud<PointT>>());
//std::vector<pcl::PointCloud<PointT>> cluster_vector{{}};
pcl::PointCloud<PointT> cluster_vector[cluster_indices.size ()];
//std::vector<pcl::PointCloud<PointT>> myRow(cluster_indices.size ());
//std::cout << "größe von cluster_vector: " << cluster_vector.size ();
pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
ec.extract (cluster_indices);
int count = 0;
pcl::PointCloud<PointT> cluster_vector[cluster_indices.size()];
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
//std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
std::vector<pcl::PointCloud<PointT>> single_cluster;
pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
for (const auto& idx : it->indices){
cloud_cluster->push_back ((*cloud)[idx]);
cloud_cluster->width = cloud_cluster->size ();
@ -274,49 +263,64 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
}
count++;
}
//std::cout << "size of first cluster that has been found " << cloud_cluster->size() << std::endl;
//std::cout << "size of first cluster that has been found " << cluster_indices.size() << std::endl;
std::cout << "size of first cluster that has been found " << sizeof(cluster_vector)/sizeof(cluster_vector[0]) << std::endl;
//std::cout << "size of first cluster that has been found " << cluster_vector.size() << std::endl;
int color_id = 1;
for (int i = 0; i < cluster_indices.size(); i++){
if (color_id == 1){
for (int l = 0; l < cluster_vector[i].points.size(); l++){
cluster_vector[i].points[l].r = 200;
cluster_vector[i].points[l].g = 0;
cluster_vector[i].points[l].b = 0;
}
visualization_msgs::Marker center_point;
uint32_t shape = visualization_msgs::Marker::SPHERE_LIST;
center_point.header.stamp = ros::Time::now();
center_point.type = shape;
center_point.action = visualization_msgs::Marker::ADD;
std::cout << "number of clusters that have been found: " << sizeof(cluster_vector)/sizeof(cluster_vector[0]) << std::endl;
//std::cout << "size of all cluster points: " << sizeof(cluster_vector) << std::endl;
//std::cout << "size of 2nd cluster: " << sizeof(cluster_vector[1]) << endl;
int color_id = 0;
//geometry_msgs::Point centroids[sizeof(cluster_vector)/sizeof(cluster_vector[0])]; //sizeof(cluster_vector)/sizeof(cluster_vector[0])
//std::vector<geometry_msgs::Point> centroids;
Eigen::Vector4f centroid;
//colored stored in rgb_colors: red, blue, green, yellow, magenta, orange, light blue, black, purple, grey, pink, light red
int rgb_colors[12][3] = {{200,0,0},{0,0,200},{0,200,0},{255,255,0},{255,0,255},{255,165,0},{102,255,255},{0,0,0},{102,0,102},{192,192,192},{255,102,178},{255,51,51}};
for (int i = 0; i < sizeof(cluster_vector)/sizeof(cluster_vector[0]); i++){
for (int l = 0; l < cluster_vector[i].points.size(); l++){ //sizeof(cluster_vector[i])
cluster_vector[i].points[l].r = rgb_colors[color_id][0];
cluster_vector[i].points[l].g = rgb_colors[color_id][1];
cluster_vector[i].points[l].b = rgb_colors[color_id][2];
}
if (color_id == 2){
for (int l = 0; l < cluster_vector[i].points.size(); l++){
cluster_vector[i].points[l].r = 0;
cluster_vector[i].points[l].g = 0;
cluster_vector[i].points[l].b = 200;
}
}
if (color_id == 3){
for (int l = 0; l < cluster_vector[i].points.size(); l++){
cluster_vector[i].points[l].r = 0;
cluster_vector[i].points[l].g = 200;
cluster_vector[i].points[l].b = 0;
color_id = 0;
}
}
std::cout << color_id;
pcl::compute3DCentroid (cluster_vector[i], centroid);
geometry_msgs::Point point;
point.x = centroid[0];
point.y = centroid[1];
point.z = centroid[2];
center_point.points.push_back(point);
color_id++;
if (color_id > sizeof(rgb_colors)/sizeof(rgb_colors[0])){
color_id = 0;
}
}
center_point.pose.orientation.w = 1.0;
center_point.scale.x = 0.1;
center_point.scale.y = 0.1;
center_point.scale.z = 0.1;
center_point.color.r = 1.0;
center_point.color.g = 1.0;
center_point.color.b = 1.0;
center_point.color.a = 1.0;
center_point.lifetime = ros::Duration();
center_point.header.frame_id = "zed2i_base_link";
steer.publish (center_point);
pcl::PointCloud<PointT> clusters;
for (int j = 0; j < cluster_indices.size(); j++){
for (int j = 0; j < sizeof(cluster_vector)/sizeof(cluster_vector[0]); j++){
clusters += cluster_vector[j];
}
sensor_msgs::PointCloud2 clustered_cloud;
pcl::toROSMsg(clusters, clustered_cloud); // publish all points in all rows
//pcl::toROSMsg(*cloud_cluster, clustered_cloud);
pcl::toROSMsg(clusters, clustered_cloud);
clustered_cloud.header.frame_id = "zed2i_base_link";
pub3.publish(clustered_cloud);
}
// convert to ROS data type
@ -347,6 +351,7 @@ int main(int argc, char **argv){
pub3 = n.advertise<sensor_msgs::PointCloud2>("clustered_cloud", 1);
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10);
steer = n.advertise<visualization_msgs::Marker>("centroid_marker", 1);
// create publisher for visualising marker
visual = n.advertise<visualization_msgs::Marker> ("marker", 1);