32 #ifndef _REGION_GROWING_NODE_HPP_
33 #define _REGION_GROWING_NODE_HPP_
37 template<
class T,
class T1>
40 typename PointCloud<T>::Ptr cloud (
new PointCloud<T> ());
41 typename PointCloud<T1>::Ptr laser_scan_ptr (
new PointCloud<T1> (laser_scan));
43 fromROSMsg(*msg,*cloud);
46 EuclideanClusterExtraction<T1> ec_inside;
47 vector<PointIndices> indices;
48 ec_inside.setClusterTolerance (clustering_tolerance);
49 ec_inside.setMinClusterSize (min_cluster_size);
50 ec_inside.setMaxClusterSize (max_cluster_size);
51 ec_inside.setInputCloud (laser_scan_ptr);
52 ec_inside.extract (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_markers_ptr->publish(marker_eraser);
65 pub_markers_text_ptr->publish(marker_eraser);
72 for (vector<PointIndices>::const_iterator it = indices.begin (); it != indices.end (); ++it)
76 typename PointCloud<T1>::Ptr cluster (
new PointCloud<T1>);
77 typename PointCloud<T>::Ptr cluster_growed (
new PointCloud<T>);
78 cluster->header.frame_id=msg->header.frame_id;
79 cluster_growed->header.frame_id=msg->header.frame_id;
80 for (vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
81 cluster->points.push_back (laser_scan_ptr->points[*pit]);
86 if ((
int)cluster->points.size()>0)
88 PointIndices::Ptr region_indices (
new PointIndices ());
99 if (region_indices->indices.size()>0)
102 ExtractIndices<T> extract;
103 extract.setInputCloud (cloud);
105 extract.setIndices (region_indices);
106 extract.setNegative (
false);
107 extract.filter (*cluster_growed);
109 visualization_msgs::Marker marker_info=mk.
mark_text(cluster_growed,
"Obstacle" ,marker_id, 0.0, 1.0, 0.0);
110 visualization_msgs::Marker marker=mk.
mark(cluster_growed,
"Obstacle" ,marker_id, 1.0, 0.0, 0.0);
111 pub_markers_ptr->publish(marker);
112 pub_markers_text_ptr->publish(marker_info);
114 cluster_growed.reset();
115 region_indices.reset();
120 laser_scan_ptr.reset();
124 template<
class T,
class T1>
128 pcl::PCLPointCloud2 pcl_pc;
129 pcl_conversions::toPCL(*msg, pcl_pc);
130 pcl::fromPCLPointCloud2(pcl_pc, laser_scan);
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)
region_growing_node class main declaration
void callback_cloud(const sensor_msgs::PointCloud2Ptr &msg)
callback for point cloud
void set_input_seeds(typename PointCloud< T1 >::Ptr seeds_in)
Sets the input point cloud seeds.
void callback_laser(const sensor_msgs::PointCloud2Ptr &msg)
callback for laser cloud
float radius
The radius for search.
PointIndices get_region_point_indices(void)
Provide acces to the result indices.
void set_input_cloud(typename PointCloud< T >::Ptr cloud_in)
Sets the input point cloud data.
void filter()
simply runs the algorithm
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...
region growing based algorithm.
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