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 geometric_shapes_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
00133 }