euclidean_cluster.hpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
32 #ifndef _EUCLIDEAN_CLUSTER_HPP_
33 #define _EUCLIDEAN_CLUSTER_HPP_
34 
35 
37 
38 template<class T>
39 void euclidean_cluster_extraction<T>::callback_cloud (const sensor_msgs::PointCloud2Ptr& msg)
40 {
41  typename PointCloud<T>::Ptr cloud (new PointCloud<T> ());
42  fromROSMsg(*msg,*cloud);
43  if ((int)cloud->points.size()>0)
44  {
45  // Euclidean segmentation
46  EuclideanClusterExtraction<T> ec_inside;
47  vector<PointIndices> inside_indices;
48  ec_inside.setClusterTolerance (cluster_tolerance); // 20cm
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);
53 
54  // Declare markers class
55  markers<T> mk;
56 
57  // Delete all previous markers
58  static int marker_id;
59  if (marker_id>0)
60  {
61  for (int i=0; i<=marker_id; ++i)
62  {
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);
66  }
67  }
68 
69  marker_id=0;
70  for (vector<PointIndices>::const_iterator it = inside_indices.begin (); it != inside_indices.end (); ++it)
71  {
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]);
76 
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);
79  marker_id++;
80  pub_marker_ptr->publish(marker);
81  pub_marker_text_ptr->publish(marker_info);
82  inside_cluster.reset();
83  }
84 
85  }
86  else
87  ROS_ERROR("EUCLIDEAN_CLUSTERING: NO DATA POINTS RECEIVED!");
88 
89  cloud.reset();
90 };
92 
93 #endif
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
void callback_cloud(const sensor_msgs::PointCloud2Ptr &msg)
callback function for pointcloud to cluster
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


pointcloud_segmentation
Author(s): Tiago Talhada
autogenerated on Mon Mar 2 2015 01:32:39