$search
00001 /********************************************************************* 00002 * 00003 * Copyright (c) 2009, Willow Garage, Inc. 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions 00008 * are met: 00009 * 00010 * * Redistributions of source code must retain the above copyright 00011 * notice, this list of conditions and the following disclaimer. 00012 * * Redistributions in binary form must reproduce the above 00013 * copyright notice, this list of conditions and the following 00014 * disclaimer in the documentation and/or other materials provided 00015 * with the distribution. 00016 * * Neither the name of the Willow Garage nor the names of its 00017 * contributors may be used to endorse or promote products derived 00018 * from this software without specific prior written permission. 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 *********************************************************************/ 00033 00034 // Author(s): Marius Muja and Matei Ciocarlie 00035 00036 #ifndef _MARKER_GENERATOR_H_ 00037 #define _MARKER_GENERATOR_H_ 00038 00039 #include <vector> 00040 00041 #include <boost/ptr_container/ptr_vector.hpp> 00042 00043 #include <visualization_msgs/Marker.h> 00044 #include <arm_navigation_msgs/Shape.h> 00045 #include <geometry_msgs/Pose.h> 00046 00047 namespace tabletop_object_detector { 00048 00049 class ModelFitInfo; 00050 00052 00055 class MarkerGenerator { 00056 public: 00058 static visualization_msgs::Marker getTableMarker(float xmin, float xmax, float ymin, float ymax); 00060 template <class PointCloudType> 00061 static visualization_msgs::Marker getCloudMarker(const PointCloudType& cloud); 00063 static visualization_msgs::Marker getFitMarker(const arm_navigation_msgs::Shape &mesh, double rank); 00065 static visualization_msgs::Marker getConvexHullTableMarker(const arm_navigation_msgs::Shape &mesh); 00067 static visualization_msgs::Marker createMarker(std::string frame_id, double duration, double xdim, double ydim, double zdim, 00068 double r, double g, double b, int type, int id, std::string ns, geometry_msgs::Pose pose); 00069 }; 00070 00075 template <class PointCloudType> 00076 visualization_msgs::Marker MarkerGenerator::getCloudMarker(const PointCloudType& cloud) 00077 { 00078 static bool first_time = true; 00079 if (first_time) { 00080 srand ( time(NULL) ); 00081 first_time = false; 00082 } 00083 00084 //create the marker 00085 visualization_msgs::Marker marker; 00086 marker.action = visualization_msgs::Marker::ADD; 00087 marker.lifetime = ros::Duration(); 00088 00089 marker.type = visualization_msgs::Marker::POINTS; 00090 marker.scale.x = 0.002; 00091 marker.scale.y = 0.002; 00092 marker.scale.z = 1.0; 00093 00094 marker.color.r = ((double)rand())/RAND_MAX; 00095 marker.color.g = ((double)rand())/RAND_MAX; 00096 marker.color.b = ((double)rand())/RAND_MAX; 00097 marker.color.a = 1.0; 00098 00099 for(size_t i=0; i<cloud.points.size(); i++) { 00100 geometry_msgs::Point p; 00101 p.x = cloud.points[i].x; 00102 p.y = cloud.points[i].y; 00103 p.z = cloud.points[i].z; 00104 marker.points.push_back(p); 00105 } 00106 00107 //the caller must decide the header; we are done here 00108 return marker; 00109 } 00110 00111 00112 }//namespace 00113 00114 #endif