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 #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
00045 #include <geometric_shapes_msgs/Shape.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 geometric_shapes_msgs::Shape &mesh, double rank);
00064 };
00065
00070 template <class PointCloudType>
00071 visualization_msgs::Marker MarkerGenerator::getCloudMarker(const PointCloudType& cloud)
00072 {
00073 static bool first_time = true;
00074 if (first_time) {
00075 srand ( time(NULL) );
00076 first_time = false;
00077 }
00078
00079
00080 visualization_msgs::Marker marker;
00081 marker.action = visualization_msgs::Marker::ADD;
00082 marker.lifetime = ros::Duration();
00083
00084 marker.type = visualization_msgs::Marker::POINTS;
00085 marker.scale.x = 0.002;
00086 marker.scale.y = 0.002;
00087 marker.scale.z = 1.0;
00088
00089 marker.color.r = ((double)rand())/RAND_MAX;
00090 marker.color.g = ((double)rand())/RAND_MAX;
00091 marker.color.b = ((double)rand())/RAND_MAX;
00092 marker.color.a = 1.0;
00093
00094 for(size_t i=0; i<cloud.points.size(); i++) {
00095 geometry_msgs::Point p;
00096 p.x = cloud.points[i].x;
00097 p.y = cloud.points[i].y;
00098 p.z = cloud.points[i].z;
00099 marker.points.push_back(p);
00100 }
00101
00102
00103 return marker;
00104 }
00105
00106
00107 }
00108
00109 #endif