32 #ifndef _EUCLIDEAN_CLUSTER_HPP_
33 #define _EUCLIDEAN_CLUSTER_HPP_
41 typename PointCloud<T>::Ptr cloud (
new PointCloud<T> ());
42 fromROSMsg(*msg,*cloud);
43 if ((
int)cloud->points.size()>0)
46 EuclideanClusterExtraction<T> ec_inside;
47 vector<PointIndices> inside_indices;
48 ec_inside.setClusterTolerance (cluster_tolerance);
49 ec_inside.setMinClusterSize (min_cluster_size);
50 ec_inside.setMaxClusterSize (max_cluster_size);
51 ec_inside.setInputCloud (cloud);
52 ec_inside.extract (inside_indices);
61 for (
int i=0; i<=marker_id; ++i)
63 visualization_msgs::Marker marker_eraser=mk.
unmark(msg->header.frame_id,
"Obstacle", i);
64 pub_marker_ptr->publish(marker_eraser);
65 pub_marker_text_ptr->publish(marker_eraser);
70 for (vector<PointIndices>::const_iterator it = inside_indices.begin (); it != inside_indices.end (); ++it)
72 typename PointCloud<T>::Ptr inside_cluster (
new PointCloud<T>);
73 inside_cluster->header.frame_id=msg->header.frame_id;
74 for (vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
75 inside_cluster->points.push_back (cloud->points[*pit]);
77 visualization_msgs::Marker marker_info=mk.
mark_text(inside_cluster,
"Obstacle" ,marker_id, 1.0, 0.0, 0.0);
78 visualization_msgs::Marker marker=mk.
mark(inside_cluster,
"Obstacle" ,marker_id, 0.0, 1.0, 0.0);
80 pub_marker_ptr->publish(marker);
81 pub_marker_text_ptr->publish(marker_info);
82 inside_cluster.reset();
87 ROS_ERROR(
"EUCLIDEAN_CLUSTERING: NO DATA POINTS RECEIVED!");
visualization_msgs::Marker mark_text(typename PointCloud< T >::Ptr cloud_cluster, string ns, int id, float r, float g, float b)
function to create a marker with some text info about a pointcloud (cluster)
euclidean_cluster_extraction class declaration
visualization_msgs::Marker unmark(std::string frame_id, std::string ns, int id)
function to estimate a marker arround the cluster to show on rviz This function will set a marker ver...
visualization_msgs::Marker mark(typename PointCloud< T >::Ptr cloud_cluster, string ns, int id, float r, float g, float b)
function to estimate a marker arround the cluster to show on rviz