00001 /************************************************************************************************** 00002 Software License Agreement (BSD License) 00003 00004 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt 00005 All rights reserved. 00006 00007 Redistribution and use in source and binary forms, with or without modification, are permitted 00008 provided that the following conditions are met: 00009 00010 *Redistributions of source code must retain the above copyright notice, this list of 00011 conditions and the following disclaimer. 00012 *Redistributions in binary form must reproduce the above copyright notice, this list of 00013 conditions and the following disclaimer in the documentation and/or other materials provided 00014 with the distribution. 00015 *Neither the name of the University of Aveiro nor the names of its contributors may be used to 00016 endorse or promote products derived from this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 00019 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 00020 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 00021 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00022 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00023 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 00024 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 00025 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00026 ***************************************************************************************************/ 00033 #include <pointcloud_segmentation/euclidean_cluster.h> 00034 00035 int main(int argc, char **argv) 00036 { 00037 // Set up ROS. 00038 ros::init(argc, argv, "euclidean_cluster_nodelet"); 00039 ros::NodeHandle n; 00040 00041 // Variables to read from launch file 00042 string pointcloud_input; 00043 string cluster_markers; 00044 string cluster_markers_text; 00045 00046 // Create the euclidean_cluster_class object 00047 euclidean_cluster_extraction<PointXYZRGB> * ec_class = new euclidean_cluster_extraction<PointXYZRGB> (); 00048 00049 // Read data from launch file 00050 ros::NodeHandle private_node_handle_("~"); 00051 private_node_handle_.param("pointcloud_input", pointcloud_input, string("pointcloud_input")); 00052 private_node_handle_.param("cluster_markers", cluster_markers, string("cluster_markers")); 00053 private_node_handle_.param("cluster_markers_text", cluster_markers_text, string("cluster_markers_text")); 00054 private_node_handle_.param("cluster_tolerance", ec_class->cluster_tolerance, double(ec_class->cluster_tolerance)); 00055 private_node_handle_.param("min_cluster_size", ec_class->min_cluster_size, double(ec_class->min_cluster_size)); 00056 private_node_handle_.param("max_cluster_size", ec_class->max_cluster_size, double(ec_class->max_cluster_size)); 00057 00058 // Subscribe messages 00059 ros::Subscriber sub_cloud = n.subscribe(pointcloud_input.c_str(), 1, &euclidean_cluster_extraction<PointXYZRGB>::callback_cloud,ec_class); 00060 00061 // Publish messages 00062 ros::Publisher pub_markers=n.advertise<visualization_msgs::Marker>(cluster_markers.c_str(),1000); 00063 ros::Publisher pub_markers_text=n.advertise<visualization_msgs::Marker>(cluster_markers_text.c_str(),1000); 00064 00065 // Set object pointers 00066 ec_class->pub_marker_ptr=&pub_markers; 00067 ec_class->pub_marker_text_ptr=&pub_markers_text; 00068 00069 // Main loop. 00070 while (n.ok()) 00071 { 00072 ros::spinOnce(); 00073 } 00074 00075 return 0; 00076 }