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 arm_navigation_msgs::Shape &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 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 }