marker_utils.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "marker_utils.h"
31 #include "rviz/default_plugin/markers/shape_marker.h"
32 #include "rviz/default_plugin/markers/arrow_marker.h"
33 #include "rviz/default_plugin/markers/line_list_marker.h"
34 #include "rviz/default_plugin/markers/line_strip_marker.h"
35 #include "rviz/default_plugin/marker_display.h"
36 #include "rviz/default_plugin/markers/points_marker.h"
37 #include "rviz/default_plugin/markers/text_view_facing_marker.h"
38 #include "rviz/default_plugin/markers/mesh_resource_marker.h"
39 #include "rviz/default_plugin/markers/triangle_list_marker.h"
40 #include "rviz/display_context.h"
41 #include "rviz/properties/property.h"
42 #include "rviz/properties/property_tree_model.h"
43 #include "rviz/properties/status_property.h"
44 #include "rviz/properties/status_list.h"
45 #include "rviz/validate_quaternions.h"
46 #include "rviz/validate_floats.h"
47 
48 namespace rviz
49 {
50 MarkerBase*
51 createMarker(int marker_type, MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node)
52 {
53  switch (marker_type)
54  {
55  case visualization_msgs::Marker::CUBE:
56  case visualization_msgs::Marker::CYLINDER:
57  case visualization_msgs::Marker::SPHERE:
58  return new rviz::ShapeMarker(owner, context, parent_node);
59 
60  case visualization_msgs::Marker::ARROW:
61  return new rviz::ArrowMarker(owner, context, parent_node);
62 
63  case visualization_msgs::Marker::LINE_STRIP:
64  return new rviz::LineStripMarker(owner, context, parent_node);
65 
66  case visualization_msgs::Marker::LINE_LIST:
67  return new rviz::LineListMarker(owner, context, parent_node);
68 
69  case visualization_msgs::Marker::SPHERE_LIST:
70  case visualization_msgs::Marker::CUBE_LIST:
71  case visualization_msgs::Marker::POINTS:
72  return new rviz::PointsMarker(owner, context, parent_node);
73 
74  case visualization_msgs::Marker::TEXT_VIEW_FACING:
75  return new rviz::TextViewFacingMarker(owner, context, parent_node);
76 
77  case visualization_msgs::Marker::MESH_RESOURCE:
78  return new rviz::MeshResourceMarker(owner, context, parent_node);
79 
80  case visualization_msgs::Marker::TRIANGLE_LIST:
81  return new rviz::TriangleListMarker(owner, context, parent_node);
82 
83  default:
84  return nullptr;
85  }
86 }
87 
88 namespace
89 {
90 void addSeparatorIfRequired(std::stringstream& ss)
91 {
92  if (ss.tellp() != 0) // check if string is not empty
93  {
94  ss << " \n";
95  }
96 }
97 
98 void increaseLevel(::ros::console::levels::Level new_status,
99  ::ros::console::levels::Level& current_status)
100 {
101  if (new_status > current_status)
102  current_status = new_status;
103 }
104 
105 template <typename T>
106 constexpr const char* fieldName();
107 template <>
108 constexpr const char* fieldName<::geometry_msgs::Point>()
109 {
110  return "Position";
111 }
112 template <>
113 constexpr const char* fieldName<::geometry_msgs::Quaternion>()
114 {
115  return "Orientation";
116 }
117 template <>
118 constexpr const char* fieldName<::geometry_msgs::Vector3>()
119 {
120  return "Scale";
121 }
122 template <>
123 constexpr const char* fieldName<::std_msgs::ColorRGBA>()
124 {
125  return "Color";
126 }
127 
128 template <typename T>
129 void checkFloats(const T& t, std::stringstream& ss, ::ros::console::levels::Level& level)
130 {
131  if (!validateFloats(t))
132  {
133  addSeparatorIfRequired(ss);
134  ss << fieldName<T>() << " contains invalid floating point values (nans or infs)";
135  increaseLevel(::ros::console::levels::Error, level);
136  }
137 }
138 
139 void checkQuaternion(const visualization_msgs::Marker& marker,
140  std::stringstream& ss,
141  ::ros::console::levels::Level& level)
142 {
143  checkFloats(marker.pose.orientation, ss, level);
144  if (marker.pose.orientation.x == 0.0 && marker.pose.orientation.y == 0.0 &&
145  marker.pose.orientation.z == 0.0 && marker.pose.orientation.w == 0.0)
146  {
147  addSeparatorIfRequired(ss);
148  ss << "Uninitialized quaternion, assuming identity.";
149  increaseLevel(::ros::console::levels::Info, level);
150  }
151  else if (!validateQuaternions(marker.pose))
152  {
153  addSeparatorIfRequired(ss);
154  ss << "Unnormalized quaternion in marker message.";
155  increaseLevel(::ros::console::levels::Warn, level);
156  }
157 }
158 
159 void checkScale(const visualization_msgs::Marker& marker,
160  std::stringstream& ss,
161  ::ros::console::levels::Level& level)
162 {
163  checkFloats(marker.scale, ss, level);
164  // for ARROW markers, scale.z is the optional head length
165  if (marker.type == visualization_msgs::Marker::ARROW && marker.scale.x != 0.0 && marker.scale.y != 0.0)
166  return;
167 
168  if (marker.scale.x == 0.0 || marker.scale.y == 0.0 || marker.scale.z == 0.0)
169  {
170  addSeparatorIfRequired(ss);
171  ss << "Scale contains 0.0 in x, y or z.";
172  increaseLevel(::ros::console::levels::Warn, level);
173  }
174 }
175 
176 void checkScaleLineStripAndList(const visualization_msgs::Marker& marker,
177  std::stringstream& ss,
178  ::ros::console::levels::Level& level)
179 {
180  if (marker.scale.x == 0.0)
181  {
182  addSeparatorIfRequired(ss);
183  ss << "Width LINE_LIST or LINE_STRIP is 0.0 (scale.x).";
184  increaseLevel(::ros::console::levels::Warn, level);
185  }
186 }
187 
188 void checkScalePoints(const visualization_msgs::Marker& marker,
189  std::stringstream& ss,
190  ::ros::console::levels::Level& level)
191 {
192  if (marker.scale.x == 0.0 || marker.scale.y == 0.0)
193  {
194  addSeparatorIfRequired(ss);
195  ss << "Width and/or height of POINTS is 0.0 (scale.x, scale.y).";
196  increaseLevel(::ros::console::levels::Warn, level);
197  }
198 }
199 
200 void checkScaleText(const visualization_msgs::Marker& marker,
201  std::stringstream& ss,
202  ::ros::console::levels::Level& level)
203 {
204  if (marker.scale.z == 0.0)
205  {
206  addSeparatorIfRequired(ss);
207  ss << "Text height of TEXT_VIEW_FACING is 0.0 (scale.z).";
208  increaseLevel(::ros::console::levels::Warn, level);
209  }
210 }
211 
212 void checkColor(const visualization_msgs::Marker& marker,
213  std::stringstream& ss,
214  ::ros::console::levels::Level& level)
215 {
216  checkFloats(marker.color, ss, level);
217  if (marker.color.a == 0.0 &&
218  // Mesh markers use a color of (0,0,0,0) to indicate that the original color of the mesh should be
219  // used as is
220  !(marker.type == visualization_msgs::Marker::MESH_RESOURCE && marker.mesh_use_embedded_materials &&
221  marker.color.r == 0.0 && marker.color.g == 0.0 && marker.color.b == 0.0))
222  {
223  addSeparatorIfRequired(ss);
224  ss << "Marker is fully transparent (color.a is 0.0).";
225  increaseLevel(::ros::console::levels::Info, level);
226  }
227 }
228 
229 void checkPointsArrow(const visualization_msgs::Marker& marker,
230  std::stringstream& ss,
231  ::ros::console::levels::Level& level)
232 {
233  if (!marker.points.empty() && marker.points.size() != 2)
234  {
235  addSeparatorIfRequired(ss);
236  ss << "Number of points for an ARROW marker should be either 0 or 2.";
237  increaseLevel(::ros::console::levels::Error, level);
238  return;
239  }
240  for (const auto& p : marker.points)
241  checkFloats(p, ss, level);
242 }
243 
244 void checkPointsNotEmpty(const visualization_msgs::Marker& marker,
245  std::stringstream& ss,
246  ::ros::console::levels::Level& level)
247 {
248  if (marker.points.empty())
249  {
250  addSeparatorIfRequired(ss);
251  ss << "Points should not be empty for specified marker type.";
252  increaseLevel(::ros::console::levels::Error, level);
253  }
254  switch (marker.type)
255  {
256  case visualization_msgs::Marker::TRIANGLE_LIST:
257  if (marker.points.size() % 3 != 0)
258  {
259  addSeparatorIfRequired(ss);
260  ss << "Number of points should be a multiple of 3 for TRIANGLE_LIST marker.";
261  increaseLevel(::ros::console::levels::Error, level);
262  }
263  break;
264  case visualization_msgs::Marker::LINE_LIST:
265  if (marker.points.size() % 2 != 0)
266  {
267  addSeparatorIfRequired(ss);
268  ss << "Number of points should be a multiple of 2 for LINE_LIST marker.";
269  increaseLevel(::ros::console::levels::Error, level);
270  }
271  break;
272  case visualization_msgs::Marker::LINE_STRIP:
273  if (marker.points.size() <= 1)
274  {
275  addSeparatorIfRequired(ss);
276  ss << "At least two points are required for a LINE_STRIP marker.";
277  increaseLevel(::ros::console::levels::Error, level);
278  }
279  break;
280  default:
281  break;
282  }
283 }
284 
285 void checkPointsEmpty(const visualization_msgs::Marker& marker,
286  std::stringstream& ss,
287  ::ros::console::levels::Level& level)
288 {
289  if (!marker.points.empty())
290  {
291  addSeparatorIfRequired(ss);
292  ss << "Non-empty points array is ignored.";
293  increaseLevel(::ros::console::levels::Warn, level);
294  }
295 }
296 
297 void checkColors(const visualization_msgs::Marker& marker,
298  std::stringstream& ss,
299  ::ros::console::levels::Level& level,
300  unsigned int multiple)
301 {
302  if (marker.colors.empty())
303  {
304  checkColor(marker, ss, level);
305  return;
306  }
307  if (marker.colors.size() != marker.points.size() &&
308  (multiple == 1 || multiple * marker.colors.size() != marker.points.size()))
309  {
310  addSeparatorIfRequired(ss);
311  ss << "Number of colors doesn't match number of points.";
312  increaseLevel(::ros::console::levels::Error, level);
313  }
314 }
315 
316 void checkColorsEmpty(const visualization_msgs::Marker& marker,
317  std::stringstream& ss,
318  ::ros::console::levels::Level& level)
319 {
320  if (!marker.colors.empty())
321  {
322  addSeparatorIfRequired(ss);
323  ss << "Non-empty colors array is ignored.";
324  increaseLevel(::ros::console::levels::Warn, level);
325  }
326 }
327 
328 void checkTextNotEmptyOrWhitespace(const visualization_msgs::Marker& marker,
329  std::stringstream& ss,
330  ::ros::console::levels::Level& level)
331 {
332  if (marker.text.find_first_not_of(" \t\n\v\f\r") == std::string::npos)
333  {
334  addSeparatorIfRequired(ss);
335  ss << "Text is empty or only consists of whitespace.";
336  increaseLevel(::ros::console::levels::Error, level);
337  }
338 }
339 
340 void checkTextEmpty(const visualization_msgs::Marker& marker,
341  std::stringstream& ss,
342  ::ros::console::levels::Level& level)
343 {
344  if (!marker.text.empty())
345  {
346  addSeparatorIfRequired(ss);
347  ss << "Non-empty text is ignored.";
348  increaseLevel(::ros::console::levels::Info, level);
349  }
350 }
351 
352 void checkMesh(const visualization_msgs::Marker& marker,
353  std::stringstream& ss,
354  ::ros::console::levels::Level& level)
355 {
356  if (marker.mesh_resource.empty())
357  {
358  addSeparatorIfRequired(ss);
359  ss << "Empty mesh_resource path.";
360  increaseLevel(::ros::console::levels::Error, level);
361  }
362 }
363 
364 void checkMeshEmpty(const visualization_msgs::Marker& marker,
365  std::stringstream& ss,
366  ::ros::console::levels::Level& level)
367 {
368  if (!marker.mesh_resource.empty())
369  {
370  addSeparatorIfRequired(ss);
371  ss << "Non-empty mesh_resource is ignored.";
372  increaseLevel(::ros::console::levels::Info, level);
373  }
374  if (marker.mesh_use_embedded_materials)
375  {
376  addSeparatorIfRequired(ss);
377  ss << "mesh_use_embedded_materials is ignored.";
378  increaseLevel(::ros::console::levels::Info, level);
379  }
380 }
381 
382 } // namespace
383 
384 bool checkMarkerMsg(const visualization_msgs::Marker& marker, MarkerDisplay* owner)
385 {
386  if (marker.action != visualization_msgs::Marker::ADD)
387  return true;
388 
389  std::stringstream ss;
390  ::ros::console::levels::Level level = ::ros::console::levels::Debug;
391  checkFloats(marker.pose.position, ss, level);
392 
393  switch (marker.type)
394  {
395  case visualization_msgs::Marker::ARROW:
396  checkQuaternion(marker, ss, level);
397  checkScale(marker, ss, level);
398  checkColor(marker, ss, level);
399  checkPointsArrow(marker, ss, level);
400  checkColorsEmpty(marker, ss, level);
401  checkTextEmpty(marker, ss, level);
402  checkMeshEmpty(marker, ss, level);
403  break;
404 
405  case visualization_msgs::Marker::CUBE:
406  case visualization_msgs::Marker::CYLINDER:
407  case visualization_msgs::Marker::SPHERE:
408  checkQuaternion(marker, ss, level);
409  checkScale(marker, ss, level);
410  checkColor(marker, ss, level);
411  checkPointsEmpty(marker, ss, level);
412  checkColorsEmpty(marker, ss, level);
413  checkTextEmpty(marker, ss, level);
414  checkMeshEmpty(marker, ss, level);
415  break;
416 
417 
418  case visualization_msgs::Marker::LINE_STRIP:
419  case visualization_msgs::Marker::LINE_LIST:
420  checkQuaternion(marker, ss, level);
421  checkScaleLineStripAndList(marker, ss, level);
422  checkPointsNotEmpty(marker, ss, level);
423  checkColors(marker, ss, level, 1);
424  checkTextEmpty(marker, ss, level);
425  checkMeshEmpty(marker, ss, level);
426  break;
427 
428  case visualization_msgs::Marker::SPHERE_LIST:
429  case visualization_msgs::Marker::CUBE_LIST:
430  case visualization_msgs::Marker::TRIANGLE_LIST:
431  checkQuaternion(marker, ss, level);
432  checkScale(marker, ss, level);
433  checkPointsNotEmpty(marker, ss, level);
434  checkColors(marker, ss, level, marker.type == visualization_msgs::Marker::TRIANGLE_LIST ? 3 : 1);
435  checkTextEmpty(marker, ss, level);
436  checkMeshEmpty(marker, ss, level);
437  break;
438 
439  case visualization_msgs::Marker::POINTS:
440  checkScalePoints(marker, ss, level);
441  checkPointsNotEmpty(marker, ss, level);
442  checkColors(marker, ss, level, 1);
443  checkTextEmpty(marker, ss, level);
444  checkMeshEmpty(marker, ss, level);
445  break;
446 
447  case visualization_msgs::Marker::TEXT_VIEW_FACING:
448  checkColor(marker, ss, level);
449  checkScaleText(marker, ss, level);
450  checkTextNotEmptyOrWhitespace(marker, ss, level);
451  checkPointsEmpty(marker, ss, level);
452  checkColorsEmpty(marker, ss, level);
453  checkMeshEmpty(marker, ss, level);
454  break;
455 
456  case visualization_msgs::Marker::MESH_RESOURCE:
457  checkQuaternion(marker, ss, level);
458  checkColor(marker, ss, level);
459  checkScale(marker, ss, level);
460  checkPointsEmpty(marker, ss, level);
461  checkColorsEmpty(marker, ss, level);
462  checkTextEmpty(marker, ss, level);
463  checkMesh(marker, ss, level);
464  break;
465 
466  default:
467  ss << "Unknown marker type: " << marker.type << '.';
468  level = ::ros::console::levels::Error;
469  }
470 
471  if (ss.tellp() != 0) // stringstream is not empty
472  {
473  std::string warning = ss.str();
474 
475  bool fatal = level >= ::ros::console::levels::Error;
476  if (owner)
477  owner->setMarkerStatus(MarkerID(marker.ns, marker.id),
478  fatal ? StatusProperty::Error : StatusProperty::Warn, warning);
479  else
480  ROS_LOG(level, ROSCONSOLE_DEFAULT_NAME ".marker", "Marker '%s/%d': %s", marker.ns.c_str(),
481  marker.id, warning.c_str());
482 
483  return !fatal;
484  }
485 
486  if (owner)
487  owner->deleteMarkerStatus(MarkerID(marker.ns, marker.id));
488  return true;
489 }
490 
491 bool checkMarkerArrayMsg(const visualization_msgs::MarkerArray& array, MarkerDisplay* owner)
492 {
493  std::vector<MarkerID> marker_ids;
494  marker_ids.reserve(array.markers.size());
495 
496  ::ros::console::levels::Level level = ::ros::console::levels::Debug;
497  std::stringstream ss;
498 
499  for (const visualization_msgs::Marker& marker : array.markers)
500  {
501  if (marker.action == visualization_msgs::Marker::DELETEALL)
502  {
503  if (!marker_ids.empty())
504  {
505  addSeparatorIfRequired(ss);
506  ss << "DELETEALL after having markers added. These will never show.";
507  increaseLevel(::ros::console::levels::Warn, level);
508  }
509  marker_ids.clear();
510  continue;
511  }
512 
513  MarkerID current_id(marker.ns, marker.id);
514  std::vector<MarkerID>::iterator search =
515  std::lower_bound(marker_ids.begin(), marker_ids.end(), current_id);
516  if (search != marker_ids.end() && *search == current_id)
517  {
518  addSeparatorIfRequired(ss);
519  increaseLevel(::ros::console::levels::Warn, level);
520  if (marker.action == visualization_msgs::Marker::DELETE)
521  {
522  ss << "Deleting just added marker '" << marker.ns.c_str() << "/" << marker.id << "'.";
523  marker_ids.erase(search);
524  }
525  else
526  ss << "Adding marker '" << marker.ns.c_str() << "/" << marker.id << "' multiple times.";
527  }
528  else
529  {
530  marker_ids.insert(search, current_id);
531  }
532  }
533 
534  if (ss.tellp() != 0) // stringstream is not empty
535  {
536  std::string warning = ss.str();
537 
538  bool fatal = level >= ::ros::console::levels::Error;
539  if (owner)
540  owner->setStatusStd(fatal ? StatusProperty::Error : StatusProperty::Warn, "marker_array", warning);
541  else
542  ROS_LOG(level, ROSCONSOLE_DEFAULT_NAME ".marker", "MarkerArray: %s", warning.c_str());
543 
544  return !fatal;
545  }
546 
547  if (owner)
548  owner->deleteStatusStd("marker_array");
549  return true;
550 }
551 
552 } // namespace rviz
rviz::Display::setStatusStd
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
Definition: display.h:163
rviz::Display::deleteStatusStd
void deleteStatusStd(const std::string &name)
Delete the status entry with the given std::string name. This is thread-safe.
Definition: display.h:172
mesh_resource_marker.h
rviz::MarkerDisplay::setMarkerStatus
void setMarkerStatus(MarkerID id, StatusLevel level, const std::string &text)
Definition: marker_display.cpp:244
points_marker.h
shape_marker.h
triangle_list_marker.h
status_list.h
rviz::checkMarkerMsg
bool checkMarkerMsg(const visualization_msgs::Marker &marker, MarkerDisplay *owner)
Definition: marker_utils.cpp:384
t
geometry_msgs::TransformStamped t
rviz::createMarker
MarkerBase * createMarker(int marker_type, MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
Definition: marker_utils.cpp:51
display_context.h
rviz::MarkerID
std::pair< std::string, int32_t > MarkerID
Definition: interactive_marker_display.h:59
rviz::checkMarkerArrayMsg
bool checkMarkerArrayMsg(const visualization_msgs::MarkerArray &array, MarkerDisplay *owner)
Definition: marker_utils.cpp:491
rviz::DisplayContext
Pure-virtual base class for objects which give Display subclasses context in which to work...
Definition: display_context.h:81
rviz::validateFloats
bool validateFloats(const sensor_msgs::CameraInfo &msg)
Definition: camera_display.cpp:70
marker_display.h
line_list_marker.h
validate_quaternions.h
rviz::MarkerDisplay::deleteMarkerStatus
void deleteMarkerStatus(MarkerID id)
Definition: marker_display.cpp:252
arrow_marker.h
ROS_LOG
#define ROS_LOG(level, name,...)
property_tree_model.h
status_property.h
marker_utils.h
line_strip_marker.h
rviz::validateQuaternions
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
Definition: interactive_marker_display.cpp:65
search
ROSCPP_DECL bool search(const std::string &ns, const std::string &key, std::string &result)
rviz::MarkerDisplay
Displays "markers" sent in by other ROS nodes on the "visualization_marker" topic.
Definition: marker_display.h:70
ROSCONSOLE_DEFAULT_NAME
#define ROSCONSOLE_DEFAULT_NAME
validate_floats.h
text_view_facing_marker.h

rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat May 27 2023 02:06:24