From 8f4ec0bf9f0be70d7d0a0349007a86d6a4eea1ad Mon Sep 17 00:00:00 2001 From: caroline Date: Mon, 2 May 2022 14:07:50 +0200 Subject: [PATCH] colored clusters --- cfg/config.cfg | 2 +- src/initial_processing.cpp | 107 +++++++++++++++++++------------------ 2 files changed, 57 insertions(+), 52 deletions(-) diff --git a/cfg/config.cfg b/cfg/config.cfg index 33020a9..6cc9a0f 100755 --- a/cfg/config.cfg +++ b/cfg/config.cfg @@ -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) diff --git a/src/initial_processing.cpp b/src/initial_processing.cpp index d137d95..0d6bbdc 100755 --- a/src/initial_processing.cpp +++ b/src/initial_processing.cpp @@ -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 - // => 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>> cluster_vector(2, std::vector>()); - //std::vector> cluster_vector{{}}; - pcl::PointCloud cluster_vector[cluster_indices.size ()]; - //std::vector> myRow(cluster_indices.size ()); - //std::cout << "größe von cluster_vector: " << cluster_vector.size (); - - - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + ec.extract (cluster_indices); + int count = 0; + pcl::PointCloud cluster_vector[cluster_indices.size()]; for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ - //std::vector::const_iterator it = cluster_indices.begin (); - std::vector> single_cluster; + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); 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 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 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("clustered_cloud", 1); marker_pub = n.advertise("visualization_marker", 10); marker_pub2 = n.advertise("visualization_marker2", 10); + steer = n.advertise("centroid_marker", 1); // create publisher for visualising marker visual = n.advertise ("marker", 1);