$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 #include "tabletop_object_detector/marker_generator.h" 00037 00038 //for random colors 00039 #include <stdlib.h> 00040 #include <time.h> 00041 00042 #include <geometry_msgs/Pose.h> 00043 00044 #include <tf/transform_datatypes.h> 00045 00046 #include "tabletop_object_detector/model_fitter.h" 00047 00048 namespace tabletop_object_detector { 00049 00050 visualization_msgs::Marker MarkerGenerator::getFitMarker(const arm_navigation_msgs::Shape &mesh, double rank) 00051 { 00052 //create the marker 00053 visualization_msgs::Marker marker; 00054 marker.action = visualization_msgs::Marker::ADD; 00055 marker.lifetime = ros::Duration(); 00056 marker.type = visualization_msgs::Marker::POINTS; 00057 //marker will be published in object frame, so its pose is identity 00058 marker.color.a = 1.0; 00059 00060 marker.color.r = rank; 00061 marker.color.g = 1 - rank; 00062 marker.color.b = 0.0; 00063 00064 marker.scale.x = 0.003; 00065 marker.scale.y = 0.003; 00066 marker.points.insert(marker.points.begin(), mesh.vertices.begin(), mesh.vertices.end()); 00067 00068 for (int i=0; i<40; i++) 00069 { 00070 geometry_msgs::Point p; 00071 p.x = 0.0005 * i; 00072 p.y = p.z = 0.0; 00073 marker.points.push_back(p); 00074 } 00075 00076 return marker; 00077 } 00078 00085 visualization_msgs::Marker MarkerGenerator::getTableMarker(float xmin, float xmax, float ymin, float ymax) 00086 { 00087 visualization_msgs::Marker marker; 00088 marker.action = visualization_msgs::Marker::ADD; 00089 marker.type = visualization_msgs::Marker::LINE_STRIP; 00090 marker.lifetime = ros::Duration(); 00091 00092 //create the marker in the table reference frame 00093 //the caller is responsible for setting the pose of the marker to match 00094 00095 marker.scale.x = 0.001; 00096 marker.scale.y = 0.001; 00097 marker.scale.z = 0.001; 00098 00099 marker.points.resize(5); 00100 marker.points[0].x = xmin; 00101 marker.points[0].y = ymin; 00102 marker.points[0].z = 0; 00103 00104 marker.points[1].x = xmin; 00105 marker.points[1].y = ymax; 00106 marker.points[1].z = 0; 00107 00108 marker.points[2].x = xmax; 00109 marker.points[2].y = ymax; 00110 marker.points[2].z = 0; 00111 00112 marker.points[3].x = xmax; 00113 marker.points[3].y = ymin; 00114 marker.points[3].z = 0; 00115 00116 marker.points[4].x = xmin; 00117 marker.points[4].y = ymin; 00118 marker.points[4].z = 0; 00119 00120 marker.points.resize(6); 00121 marker.points[5].x = xmin; 00122 marker.points[5].y = ymin; 00123 marker.points[5].z = 0.02; 00124 00125 marker.color.r = 1.0; 00126 marker.color.g = 1.0; 00127 marker.color.b = 0.0; 00128 marker.color.a = 1.0; 00129 00130 return marker; 00131 } 00132 00137 visualization_msgs::Marker MarkerGenerator::getConvexHullTableMarker(const arm_navigation_msgs::Shape &mesh) 00138 { 00139 visualization_msgs::Marker marker; 00140 marker.action = visualization_msgs::Marker::ADD; 00141 marker.lifetime = ros::Duration(); 00142 marker.type = visualization_msgs::Marker::LINE_STRIP; 00143 marker.color.r = 0.0; 00144 marker.color.g = 1.0; 00145 marker.color.b = 1.0; 00146 marker.color.a = 1.0; 00147 marker.scale.x = 0.001; 00148 marker.scale.y = 0.001; 00149 marker.scale.z = 0.001; 00150 for(size_t i=0; i<mesh.triangles.size(); i++) 00151 { 00152 marker.points.push_back(mesh.vertices[mesh.triangles[i]]); 00153 } 00154 return marker; 00155 } 00156 00157 00160 visualization_msgs::Marker MarkerGenerator::createMarker(std::string frame_id, double duration, double xdim, double ydim, double zdim, 00161 double r, double g, double b, int type, int id, std::string ns, geometry_msgs::Pose pose) 00162 { 00163 visualization_msgs::Marker marker; 00164 marker.action = visualization_msgs::Marker::ADD; 00165 marker.lifetime = ros::Duration(duration); 00166 marker.type = type; 00167 marker.id = id; 00168 marker.ns = ns; 00169 marker.pose = pose; 00170 marker.header.frame_id = frame_id; 00171 marker.color.r = r; 00172 marker.color.g = g; 00173 marker.color.b = b; 00174 marker.color.a = 1.0; 00175 marker.scale.x = xdim; 00176 marker.scale.y = ydim; 00177 marker.scale.z = zdim; 00178 return marker; 00179 } 00180 00181 00182 } //namespace tabletop_object_detector