Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #ifndef MARKER_LIST_H_
00009 #define MARKER_LIST_H_
00010
00011 #include <visualization_msgs/Marker.h>
00012
00013 namespace cob_3d_marker {
00014
00015 class MarkerContainer;
00016
00017 class MarkerList {
00018 int id_;
00019
00020 protected:
00021
00022 visualization_msgs::Marker marker_tmpl_;
00023 std::vector<visualization_msgs::Marker> marker_;
00024
00025 static int getNewId() {
00026 static int id=0;
00027 return ++id;
00028 }
00029
00030 public:
00031 MarkerList(const int id_) : id_(id_)
00032 {
00033
00034 marker_tmpl_.header.frame_id = "/openni_rgb_optical_frame";
00035 marker_tmpl_.pose.position.x = 0;
00036 marker_tmpl_.pose.position.y = 0;
00037 marker_tmpl_.pose.position.z = 0;
00038 marker_tmpl_.pose.orientation.x = 0.0;
00039 marker_tmpl_.pose.orientation.y = 0.0;
00040 marker_tmpl_.pose.orientation.z = 0.0;
00041 marker_tmpl_.pose.orientation.w = 1.0;
00042 marker_tmpl_.action = visualization_msgs::Marker::ADD;
00043 marker_tmpl_.color.r = marker_tmpl_.color.g = marker_tmpl_.color.b = marker_tmpl_.color.a = 1;
00044 marker_tmpl_.scale.x = marker_tmpl_.scale.y = marker_tmpl_.scale.z = 0.01;
00045 marker_tmpl_.id = getNewId();
00046 }
00047
00048 int getId() const {return id_;}
00049
00050 friend class MarkerContainer;
00051 };
00052
00053 class MarkerList_Line : public MarkerList {
00054 public:
00055
00056 MarkerList_Line(const int id):MarkerList(id) {
00057 marker_tmpl_.type = visualization_msgs::Marker::LINE_LIST;
00058 marker_.push_back(marker_tmpl_);
00059 }
00060
00061 void addLine(const Eigen::Vector3f &va, const Eigen::Vector3f &vb, const float r=1, const float g=1, const float b=1, const float a=1) {
00062 geometry_msgs::Point line_p;
00063 ::std_msgs::ColorRGBA col;
00064
00065 col.r = r;
00066 col.g = g;
00067 col.b = b;
00068 col.a = a;
00069 line_p.x = va(0);
00070 line_p.y = va(1);
00071 line_p.z = va(2);
00072 marker_.back().points.push_back(line_p);
00073 marker_.back().colors.push_back(col);
00074
00075 line_p.x = vb(0);
00076 line_p.y = vb(1);
00077 line_p.z = vb(2);
00078 marker_.back().points.push_back(line_p);
00079 marker_.back().colors.push_back(col);
00080 }
00081
00082 };
00083
00084 class MarkerList_Triangles : public MarkerList {
00085 public:
00086
00087 MarkerList_Triangles(const int id):MarkerList(id) {
00088 marker_tmpl_.type = visualization_msgs::Marker::TRIANGLE_LIST;
00089 marker_tmpl_.scale.x=marker_tmpl_.scale.y=marker_tmpl_.scale.z=1;
00090 marker_.push_back(marker_tmpl_);
00091 }
00092
00093 void addTriangle(const Eigen::Vector3f &va, const Eigen::Vector3f &vb, const Eigen::Vector3f &vc, const float r=1, const float g=1, const float b=1, const float a=1) {
00094 geometry_msgs::Point line_p;
00095 ::std_msgs::ColorRGBA col;
00096
00097 col.r = r;
00098 col.g = g;
00099 col.b = b;
00100 col.a = a;
00101 line_p.x = va(0);
00102 line_p.y = va(1);
00103 line_p.z = va(2);
00104 marker_.back().points.push_back(line_p);
00105 marker_.back().colors.push_back(col);
00106
00107 line_p.x = vb(0);
00108 line_p.y = vb(1);
00109 line_p.z = vb(2);
00110 marker_.back().points.push_back(line_p);
00111 marker_.back().colors.push_back(col);
00112
00113 line_p.x = vc(0);
00114 line_p.y = vc(1);
00115 line_p.z = vc(2);
00116 marker_.back().points.push_back(line_p);
00117 marker_.back().colors.push_back(col);
00118 }
00119
00120 };
00121
00122 class MarkerList_Arrow : public MarkerList {
00123 public:
00124
00125 MarkerList_Arrow(const int id):MarkerList(id)
00126 {
00127 marker_tmpl_.type = visualization_msgs::Marker::ARROW;
00128 }
00129
00130 void addArrow(const Eigen::Vector3f &va, const Eigen::Vector3f &vb, const float r=1, const float g=1, const float b=1, const float a=1) {
00131 visualization_msgs::Marker m = marker_tmpl_;
00132
00133 geometry_msgs::Point line_p;
00134 ::std_msgs::ColorRGBA col;
00135
00136 m.scale.x = 0.025;
00137 m.scale.y = 0.05;
00138 m.scale.z = 0.1;
00139
00140 m.color.r = r;
00141 m.color.g = g;
00142 m.color.b = b;
00143 m.color.a = a;
00144
00145 line_p.x = va(0);
00146 line_p.y = va(1);
00147 line_p.z = va(2);
00148 m.points.push_back(line_p);
00149
00150 line_p.x = vb(0);
00151 line_p.y = vb(1);
00152 line_p.z = vb(2);
00153 m.points.push_back(line_p);
00154
00155 m.id = getNewId();
00156
00157 marker_.push_back(m);
00158 }
00159
00160 };
00161
00162 class MarkerList_Text : public MarkerList {
00163 public:
00164
00165 MarkerList_Text(const int id):MarkerList(id)
00166 {
00167 marker_tmpl_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00168 }
00169
00170 void addText(const Eigen::Vector3f &pos, const std::string &text, const float size=0.1f, const float r=1, const float g=1, const float b=1, const float a=1) {
00171 visualization_msgs::Marker m = marker_tmpl_;
00172
00173 m.pose.position.x = pos(0);
00174 m.pose.position.y = pos(1);
00175 m.pose.position.z = pos(2);
00176
00177 m.text = text;
00178
00179 m.scale.z = size;
00180
00181 m.id = getNewId();
00182
00183 marker_.push_back(m);
00184 }
00185
00186 };
00187
00188 }
00189
00190
00191
00192 #endif