Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "tabletop_object_detector/marker_generator.h"
00037
00038
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 shape_msgs::Mesh &mesh, double rank)
00051 {
00052
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
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
00093
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 shape_msgs::Mesh &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].vertex_indices[0] ] );
00153 marker.points.push_back( mesh.vertices[ mesh.triangles[i].vertex_indices[1] ] );
00154 marker.points.push_back( mesh.vertices[ mesh.triangles[i].vertex_indices[2] ] );
00155 }
00156 return marker;
00157 }
00158
00159
00162 visualization_msgs::Marker MarkerGenerator::createMarker(std::string frame_id, double duration, double xdim, double ydim, double zdim,
00163 double r, double g, double b, int type, int id, std::string ns, geometry_msgs::Pose pose)
00164 {
00165 visualization_msgs::Marker marker;
00166 marker.action = visualization_msgs::Marker::ADD;
00167 marker.lifetime = ros::Duration(duration);
00168 marker.type = type;
00169 marker.id = id;
00170 marker.ns = ns;
00171 marker.pose = pose;
00172 marker.header.frame_id = frame_id;
00173 marker.color.r = r;
00174 marker.color.g = g;
00175 marker.color.b = b;
00176 marker.color.a = 1.0;
00177 marker.scale.x = xdim;
00178 marker.scale.y = ydim;
00179 marker.scale.z = zdim;
00180 return marker;
00181 }
00182
00183
00184 }