MarkerHelper.cpp
Go to the documentation of this file.
1 
20 #include "next_best_view/helper/MarkerHelper.hpp"
21 
22 namespace next_best_view {
23  MarkerHelper::MarkerHelper() { }
24 
25  MarkerHelper::~MarkerHelper() { }
26 
27  visualization_msgs::Marker MarkerHelper::getBasicMarker(int id, std::string ns) {
28  visualization_msgs::Marker marker;
29  marker.header.frame_id = "/map";
30  marker.header.stamp = ros::Time();
31  marker.lifetime = ros::Duration();
32  marker.ns = ns;
33  marker.id = id;
34  marker.action = visualization_msgs::Marker::ADD;
35  return marker;
36  }
37 
38  visualization_msgs::Marker MarkerHelper::getBasicMarker(int id, SimpleVector3 position, SimpleQuaternion orientation,
39  SimpleVector3 scale, SimpleVector4 color, std::string ns) {
40  visualization_msgs::Marker marker = getBasicMarker(id, ns);
41 
42  marker.pose.position = TypeHelper::getPointMSG(position);
43  marker.pose.orientation = TypeHelper::getQuaternionMSG(orientation);
44  marker.scale = TypeHelper::getVector3(scale);
45  marker.color = TypeHelper::getColor(color);
46 
47  return marker;
48  }
49 
50  visualization_msgs::Marker MarkerHelper::getTextMarker(int id, std::string text, geometry_msgs::Pose pose, std::string ns)
51  {
52  visualization_msgs::Marker textMarker = getBasicMarker(id, ns);
53  textMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
54  textMarker.color.a = 1;
55  textMarker.color.r = 1;
56  textMarker.color.g = 1;
57  textMarker.color.b = 1;
58  textMarker.text = text;
59  textMarker.pose = pose;
60  textMarker.scale.z = 0.25;
61  return textMarker;
62  }
63 
64  visualization_msgs::Marker MarkerHelper::getDeleteMarker(int id, std::string ns) {
65  visualization_msgs::Marker marker = getBasicMarker(id, ns);
66 
67  marker.action = visualization_msgs::Marker::DELETE;
68 
69  return marker;
70  }
71 
72  visualization_msgs::Marker MarkerHelper::getMeshMarker(int id, std::string meshResource, SimpleVector3 centroid, SimpleQuaternion quaternion,
73  SimpleVector3 scale, std::string ns) {
74  visualization_msgs::Marker marker = getBasicMarker(id, ns);
75 
76  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
77  marker.mesh_resource = meshResource;
78  marker.mesh_use_embedded_materials = true;
79  marker.pose.position = TypeHelper::getPointMSG(centroid);
80  marker.pose.orientation = TypeHelper::getQuaternionMSG(quaternion);
81  marker.scale = TypeHelper::getVector3(scale);
82 
83  return marker;
84  }
85 
86  visualization_msgs::Marker MarkerHelper::getArrowMarker(int id, SimpleVector3 startPoint, SimpleVector3 endPoint, SimpleVector3 scale,
87  SimpleVector4 color, std::string ns) {
88  visualization_msgs::Marker lmarker = getBasicMarker(id, ns);
89 
90  lmarker.type = visualization_msgs::Marker::ARROW;
91  lmarker.pose.position.x = 0;
92  lmarker.pose.position.y = 0;
93  lmarker.pose.position.z = 0;
94  lmarker.pose.orientation.x = 0.0;
95  lmarker.pose.orientation.y = 0.0;
96  lmarker.pose.orientation.z = 0.0;
97  lmarker.pose.orientation.w = 1.0;
98  // the model size unit is mm
99  lmarker.scale.x = scale[0];
100  lmarker.scale.y = scale[1];
101  lmarker.scale.z = scale[2];
102 
103  lmarker.color.r = color[0];
104  lmarker.color.g = color[1];
105  lmarker.color.b = color[2];
106  lmarker.color.a = color[3];
107 
108  lmarker.points.push_back(TypeHelper::getPointMSG(startPoint));
109  lmarker.points.push_back(TypeHelper::getPointMSG(endPoint));
110 
111  return lmarker;
112  }
113 
114  visualization_msgs::Marker MarkerHelper::getArrowMarker(int id, SimpleVector3 position, SimpleQuaternion orientation,
115  SimpleVector3 scale, SimpleVector4 color, std::string ns) {
116  visualization_msgs::Marker marker = getBasicMarker(id, position, orientation, scale, color, ns);
117 
118  marker.type = visualization_msgs::Marker::ARROW;
119 
120  return marker;
121  }
122 
123  visualization_msgs::Marker MarkerHelper::getCubeMarker(int id, SimpleVector3 position, SimpleQuaternion orientation,
124  SimpleVector3 scale, SimpleVector4 color, std::string ns) {
125  visualization_msgs::Marker marker = getBasicMarker(id, position, orientation, scale, color, ns);
126 
127  marker.type = visualization_msgs::Marker::CUBE;
128 
129  return marker;
130  }
131 
132  visualization_msgs::Marker MarkerHelper::getSphereMarker(int id, SimpleVector3 position, SimpleVector3 scale,
133  SimpleVector4 color, std::string ns) {
134  visualization_msgs::Marker marker = getBasicMarker(id, position, SimpleQuaternion(), scale, color, ns);
135 
136  marker.type = visualization_msgs::Marker::SPHERE;
137 
138  return marker;
139  }
140 
141  visualization_msgs::Marker MarkerHelper::getLineListMarker(int id, std::vector<SimpleVector3> points, double scale,
142  SimpleVector4 color, std::string ns) {
143  visualization_msgs::Marker marker = getBasicMarker(id, ns);
144 
145  marker.type = visualization_msgs::Marker::LINE_LIST;
146 
147  BOOST_FOREACH(SimpleVector3 point, points) {
148  marker.points.push_back(TypeHelper::getPointMSG(point));
149  }
150 
151  marker.scale.x = scale;
152  marker.color = TypeHelper::getColor(color);
153 
154  return marker;
155  }
156 
157  visualization_msgs::Marker MarkerHelper::getCylinderMarker(int id, SimpleVector3 position, double w, SimpleVector3 scale,
158  SimpleVector4 color, std::string ns) {
159  visualization_msgs::Marker marker = getBasicMarker(id, position, SimpleQuaternion(0, 0, 0, w), scale, color, ns);
160 
161  marker.type = visualization_msgs::Marker::CYLINDER;
162 
163  return marker;
164  }
165 
166  visualization_msgs::Marker MarkerHelper::getTriangleListMarker(int id, std::vector<SimpleVector3> vertices,
167  std::vector<SimpleVector4> colors, std::string ns)
168  {
169  visualization_msgs::Marker marker = getBasicMarker(id, SimpleVector3(0,0,0), SimpleQuaternion(1,0,0,0),
170  SimpleVector3(1,1,1), SimpleVector4(0,0,0,1), ns);
171 
172  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
173 
174  for (unsigned int i = 0; i < vertices.size(); i++)
175  {
176  marker.points.push_back(TypeHelper::getPointMSG(vertices[i]));
177  marker.colors.push_back(TypeHelper::getColor(colors[i]));
178  }
179 
180  return marker;
181  }
182 
183  void MarkerHelper::getRainbowColor(visualization_msgs::Marker &marker, double x, double alpha) {
184  // clamping value to [0.0, 1.0):125
185  if (x >= 1.0) {
186  while(x >= 1.0) {
187  x -= 1.0;
188  }
189  } else if (x < 0.0) {
190  while(x < 0.0) {
191  x += 1.0;
192  }
193  }
194 
195  double red = 0.0;
196  double green = 0.0;
197  double blue = 0.0;
198 
199  x = x * 6.0;
200  if (x < 1.0) {
201  red = 1.0;
202  green = x;
203  blue = 0.0;
204  } else if (x < 2.0) {
205  x -= 1.0;
206  red = 1 - x;
207  green = 1.0;
208  blue = 0.0;
209  } else if (x < 3.0) {
210  x -= 2.0;
211  red = 0.0;
212  green = 1.0;
213  blue = x;
214  } else if (x < 4.0) {
215  x -= 3.0;
216  red = 0.0;
217  green = 1.0 - x;
218  blue = 1.0;
219  } else if (x < 5.0) {
220  x -= 4.0;
221  red = x;
222  green = 0.0;
223  blue = 1.0;
224  } else if (x < 6.0) {
225  x -= 5.0;
226  red = 1.0;
227  green = 0.0;
228  blue = 1.0 - x;
229  }
230 
231  marker.color.r = red;
232  marker.color.g = green;
233  marker.color.b = blue;
234  marker.color.a = alpha;
235  }
236 }
237 
next_best_view::TypeHelper::getColor
static std_msgs::ColorRGBA getColor(const SimpleVector4 &vector)
Definition: TypeHelper.cpp:87
next_best_view::MarkerHelper::getTriangleListMarker
static visualization_msgs::Marker getTriangleListMarker(int id, std::vector< SimpleVector3 > vertices, std::vector< SimpleVector4 > colors, std::string ns="my_namespace")
Definition: MarkerHelper.cpp:166
next_best_view::MarkerHelper::getDeleteMarker
static visualization_msgs::Marker getDeleteMarker(int id, std::string ns="my_namespace")
returns a marker which will delete the corresponding marker with the same id and namespace ...
Definition: MarkerHelper.cpp:64
next_best_view::MarkerHelper::getCubeMarker
static visualization_msgs::Marker getCubeMarker(int id, SimpleVector3 position, SimpleQuaternion orientation, SimpleVector3 scale, SimpleVector4 color, std::string ns="my_namespace")
returns a cube marker with the given settings
Definition: MarkerHelper.cpp:123
next_best_view::SimpleVector3
Eigen::Matrix< Precision, 3, 1 > SimpleVector3
Definition: typedef.hpp:53
next_best_view::TypeHelper::getPointMSG
static geometry_msgs::Point getPointMSG(const SimpleVector3 &vector)
Definition: TypeHelper.cpp:23
next_best_view::MarkerHelper::getCylinderMarker
static visualization_msgs::Marker getCylinderMarker(int id, SimpleVector3 position, double w, SimpleVector3 scale, SimpleVector4 color, std::string ns="my_namespace")
returns a cylinder marker with the given settings
Definition: MarkerHelper.cpp:157
next_best_view::TypeHelper::getQuaternionMSG
static geometry_msgs::Quaternion getQuaternionMSG(const SimpleQuaternion &quaternion)
Definition: TypeHelper.cpp:33
next_best_view::SimpleVector4
Eigen::Matrix< Precision, 4, 1 > SimpleVector4
Definition: typedef.hpp:55
next_best_view::MarkerHelper::MarkerHelper
MarkerHelper()
Definition: MarkerHelper.cpp:23
next_best_view
this namespace contains all generally usable classes.
Definition: CameraModelFilter.hpp:26
next_best_view::MarkerHelper::getBasicMarker
static visualization_msgs::Marker getBasicMarker(int id, std::string ns="my_namespace")
returns a marker with basic settings
Definition: MarkerHelper.cpp:27
next_best_view::MarkerHelper::getArrowMarker
static visualization_msgs::Marker getArrowMarker(int id, SimpleVector3 startPoint, SimpleVector3 endPoint, SimpleVector3 scale=SimpleVector3(0.025, 0.05, 0.05), SimpleVector4 color=SimpleVector4(1.0, 0.0, 0.0, 1.0), std::string ns="my_namespace")
returns an arrow marker with the given settings
Definition: MarkerHelper.cpp:86
next_best_view::MarkerHelper::getRainbowColor
static void getRainbowColor(visualization_msgs::Marker &marker, double x, double alpha=1.0)
Definition: MarkerHelper.cpp:183
next_best_view::MarkerHelper::getLineListMarker
static visualization_msgs::Marker getLineListMarker(int id, std::vector< SimpleVector3 > points, double scale, SimpleVector4 color, std::string ns="my_namespace")
returns a line list marker with the given settings
Definition: MarkerHelper.cpp:141
next_best_view::SimpleQuaternion
Eigen::Quaternion< Precision > SimpleQuaternion
Definition: typedef.hpp:67
next_best_view::MarkerHelper::~MarkerHelper
~MarkerHelper()
Definition: MarkerHelper.cpp:25
next_best_view::MarkerHelper::getTextMarker
static visualization_msgs::Marker getTextMarker(int id, std::string text, geometry_msgs::Pose pose, std::string ns="my_namespace")
returns a text marker with the given settings
Definition: MarkerHelper.cpp:50
next_best_view::TypeHelper::getVector3
static geometry_msgs::Vector3 getVector3(const SimpleVector3 &vector)
Definition: TypeHelper.cpp:77
next_best_view::MarkerHelper::getSphereMarker
static visualization_msgs::Marker getSphereMarker(int id, SimpleVector3 position, SimpleVector3 scale, SimpleVector4 color, std::string ns="my_namespace")
returns a sphere marker with the given settings
Definition: MarkerHelper.cpp:132
next_best_view::MarkerHelper::getMeshMarker
static visualization_msgs::Marker getMeshMarker(int id, std::string meshResource, SimpleVector3 centroid, SimpleQuaternion quaternion, SimpleVector3 scale=SimpleVector3(0.001, 0.001, 0.001), std::string ns="my_namespace")
returns a mesh marker with the given settings
Definition: MarkerHelper.cpp:72

asr_next_best_view
Author(s): Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
autogenerated on Thu Jan 9 2020 07:20:18