path_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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 <boost/bind.hpp>
31 
32 #include <OgreSceneNode.h>
33 #include <OgreSceneManager.h>
34 #include <OgreManualObject.h>
35 #include <OgreMatrix4.h>
36 
37 #include "rviz/display_context.h"
38 #include "rviz/frame_manager.h"
44 #include "rviz/validate_floats.h"
46 
49 
50 namespace rviz
51 {
53 {
55  new EnumProperty("Line Style", "Lines", "The rendering operation to use to draw the grid lines.",
56  this, SLOT(updateStyle()));
57 
58  style_property_->addOption("Lines", LINES);
59  style_property_->addOption("Billboards", BILLBOARDS);
60 
62  "Line Width", 0.03,
63  "The width, in meters, of each path line. Only works with the 'Billboards' style.", this,
64  SLOT(updateLineWidth()), this);
67 
68  color_property_ = new ColorProperty("Color", QColor(25, 255, 0), "Color to draw the path.", this);
69 
71  new FloatProperty("Alpha", 1.0, "Amount of transparency to apply to the path.", this);
72 
73  buffer_length_property_ = new IntProperty("Buffer Length", 1, "Number of paths to display.", this,
74  SLOT(updateBufferLength()));
76 
78  "Offset", Ogre::Vector3::ZERO,
79  "Allows you to offset the path from the origin of the reference frame. In meters.", this,
80  SLOT(updateOffset()));
81 
82  pose_style_property_ = new EnumProperty("Pose Style", "None", "Shape to display the pose as.", this,
83  SLOT(updatePoseStyle()));
87 
88  pose_axes_length_property_ = new rviz::FloatProperty("Length", 0.3, "Length of the axes.", this,
89  SLOT(updatePoseAxisGeometry()));
90  pose_axes_radius_property_ = new rviz::FloatProperty("Radius", 0.03, "Radius of the axes.", this,
91  SLOT(updatePoseAxisGeometry()));
92 
94  new ColorProperty("Pose Color", QColor(255, 85, 255), "Color to draw the poses.", this,
95  SLOT(updatePoseArrowColor()));
97  "Shaft Length", 0.1, "Length of the arrow shaft.", this, SLOT(updatePoseArrowGeometry()));
99  "Head Length", 0.2, "Length of the arrow head.", this, SLOT(updatePoseArrowGeometry()));
101  "Shaft Diameter", 0.1, "Diameter of the arrow shaft.", this, SLOT(updatePoseArrowGeometry()));
103  "Head Diameter", 0.3, "Diameter of the arrow head.", this, SLOT(updatePoseArrowGeometry()));
111 }
112 
114 {
115  destroyObjects();
118 }
119 
121 {
124 }
125 
127 {
128  MFDClass::reset();
130 }
131 
132 
133 void PathDisplay::allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, int num)
134 {
135  if (num > axes_vect.size())
136  {
137  for (size_t i = axes_vect.size(); i < num; i++)
138  {
139  rviz::Axes* axes =
142  axes_vect.push_back(axes);
143  }
144  }
145  else if (num < axes_vect.size())
146  {
147  for (int i = axes_vect.size() - 1; num <= i; i--)
148  {
149  delete axes_vect[i];
150  }
151  axes_vect.resize(num);
152  }
153 }
154 
155 void PathDisplay::allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, int num)
156 {
157  if (num > arrow_vect.size())
158  {
159  for (size_t i = arrow_vect.size(); i < num; i++)
160  {
162  arrow_vect.push_back(arrow);
163  }
164  }
165  else if (num < arrow_vect.size())
166  {
167  for (int i = arrow_vect.size() - 1; num <= i; i--)
168  {
169  delete arrow_vect[i];
170  }
171  arrow_vect.resize(num);
172  }
173 }
174 
176 {
177  for (size_t i = 0; i < axes_chain_.size(); i++)
178  {
180  }
181  axes_chain_.resize(0);
182 }
183 
185 {
186  for (size_t i = 0; i < arrow_chain_.size(); i++)
187  {
189  }
190  arrow_chain_.resize(0);
191 }
192 
194 {
196 
197  switch (style)
198  {
199  case LINES:
200  default:
202  break;
203 
204  case BILLBOARDS:
206  break;
207  }
208 
210 }
211 
213 {
215  float line_width = line_width_property_->getFloat();
216 
217  if (style == BILLBOARDS)
218  {
219  for (size_t i = 0; i < billboard_lines_.size(); i++)
220  {
221  rviz::BillboardLine* billboard_line = billboard_lines_[i];
222  if (billboard_line)
223  billboard_line->setLineWidth(line_width);
224  }
225  }
227 }
228 
230 {
231  scene_node_->setPosition(offset_property_->getVector());
233 }
234 
236 {
238  switch (pose_style)
239  {
240  case AXES:
248  break;
249  case ARROWS:
257  break;
258  default:
266  }
268 }
269 
271 {
272  for (size_t i = 0; i < axes_chain_.size(); i++)
273  {
274  std::vector<rviz::Axes*>& axes_vect = axes_chain_[i];
275  for (size_t j = 0; j < axes_vect.size(); j++)
276  {
278  }
279  }
281 }
282 
284 {
285  QColor color = pose_arrow_color_property_->getColor();
286 
287  for (size_t i = 0; i < arrow_chain_.size(); i++)
288  {
289  std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
290  for (size_t j = 0; j < arrow_vect.size(); j++)
291  {
292  arrow_vect[j]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
293  }
294  }
296 }
297 
299 {
300  for (size_t i = 0; i < arrow_chain_.size(); i++)
301  {
302  std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
303  for (size_t j = 0; j < arrow_vect.size(); j++)
304  {
305  arrow_vect[j]->set(pose_arrow_shaft_length_property_->getFloat(),
309  }
310  }
312 }
313 
315 {
316  // Destroy all simple lines, if any
317  for (size_t i = 0; i < manual_objects_.size(); i++)
318  {
319  Ogre::ManualObject*& manual_object = manual_objects_[i];
320  if (manual_object)
321  {
322  manual_object->clear();
323  scene_manager_->destroyManualObject(manual_object);
324  manual_object = nullptr; // ensure it doesn't get destroyed again
325  }
326  }
327 
328  // Destroy all billboards, if any
329  for (size_t i = 0; i < billboard_lines_.size(); i++)
330  {
331  rviz::BillboardLine*& billboard_line = billboard_lines_[i];
332  if (billboard_line)
333  {
334  delete billboard_line; // also destroys the corresponding scene node
335  billboard_line = nullptr; // ensure it doesn't get destroyed again
336  }
337  }
338 }
339 
341 {
342  // Delete old path objects
343  destroyObjects();
344 
345  // Destroy all axes and arrows
348 
349  // Read options
350  int buffer_length = buffer_length_property_->getInt();
352 
353  // Create new path objects
354  switch (style)
355  {
356  case LINES: // simple lines with fixed width of 1px
357  manual_objects_.resize(buffer_length);
358  for (size_t i = 0; i < manual_objects_.size(); i++)
359  {
360  Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
361  manual_object->setDynamic(true);
362  scene_node_->attachObject(manual_object);
363 
364  manual_objects_[i] = manual_object;
365  }
366  break;
367 
368  case BILLBOARDS: // billboards with configurable width
369  billboard_lines_.resize(buffer_length);
370  for (size_t i = 0; i < billboard_lines_.size(); i++)
371  {
373  billboard_lines_[i] = billboard_line;
374  }
375  break;
376  }
377  axes_chain_.resize(buffer_length);
378  arrow_chain_.resize(buffer_length);
379 }
380 
381 bool validateFloats(const nav_msgs::Path& msg)
382 {
383  bool valid = true;
384  valid = valid && validateFloats(msg.poses);
385  return valid;
386 }
387 
388 void PathDisplay::processMessage(const nav_msgs::Path::ConstPtr& msg)
389 {
390  // Calculate index of oldest element in cyclic buffer
391  size_t bufferIndex = messages_received_ % buffer_length_property_->getInt();
392 
394  Ogre::ManualObject* manual_object = nullptr;
395  rviz::BillboardLine* billboard_line = nullptr;
396 
397  // Delete oldest element
398  switch (style)
399  {
400  case LINES:
401  manual_object = manual_objects_[bufferIndex];
402  manual_object->clear();
403  break;
404 
405  case BILLBOARDS:
406  billboard_line = billboard_lines_[bufferIndex];
407  billboard_line->clear();
408  break;
409  }
410 
411  // Check if path contains invalid coordinate values
412  if (!validateFloats(*msg))
413  {
415  "Message contained invalid floating point values (nans or infs)");
416  return;
417  }
418 
419  if (!validateQuaternions(msg->poses))
420  {
421  ROS_WARN_ONCE_NAMED("quaternions",
422  "Path '%s' contains unnormalized quaternions. "
423  "This warning will only be output once but may be true for others; "
424  "enable DEBUG messages for ros.rviz.quaternions to see more details.",
425  qPrintable(getName()));
426  ROS_DEBUG_NAMED("quaternions", "Path '%s' contains unnormalized quaternions.", qPrintable(getName()));
427  }
428 
429  // Lookup transform into fixed frame
430  Ogre::Vector3 position;
431  Ogre::Quaternion orientation;
432  if (!context_->getFrameManager()->getTransform(msg->header, position, orientation))
433  {
434  ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(),
435  qPrintable(fixed_frame_));
436  }
437 
438  Ogre::Matrix4 transform(orientation);
439  transform.setTrans(position);
440 
441  // scene_node_->setPosition( position );
442  // scene_node_->setOrientation( orientation );
443 
444  Ogre::ColourValue color = color_property_->getOgreColor();
445  color.a = alpha_property_->getFloat();
446 
447  uint32_t num_points = msg->poses.size();
448  float line_width = line_width_property_->getFloat();
449 
450  switch (style)
451  {
452  case LINES:
453  manual_object->estimateVertexCount(num_points);
454  manual_object->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
455  for (uint32_t i = 0; i < num_points; ++i)
456  {
457  const geometry_msgs::Point& pos = msg->poses[i].pose.position;
458  Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
459  manual_object->position(xpos.x, xpos.y, xpos.z);
460  manual_object->colour(color);
461  }
462 
463  manual_object->end();
464  break;
465 
466  case BILLBOARDS:
467  billboard_line->setNumLines(1);
468  billboard_line->setMaxPointsPerLine(num_points);
469  billboard_line->setLineWidth(line_width);
470 
471  for (uint32_t i = 0; i < num_points; ++i)
472  {
473  const geometry_msgs::Point& pos = msg->poses[i].pose.position;
474  Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
475  billboard_line->addPoint(xpos, color);
476  }
477 
478  break;
479  }
480 
481  // process pose markers
483  std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[bufferIndex];
484  std::vector<rviz::Axes*>& axes_vect = axes_chain_[bufferIndex];
485 
486  switch (pose_style)
487  {
488  case AXES:
489  allocateAxesVector(axes_vect, num_points);
490  for (uint32_t i = 0; i < num_points; ++i)
491  {
492  const geometry_msgs::Point& pos = msg->poses[i].pose.position;
493  const geometry_msgs::Quaternion& quat = msg->poses[i].pose.orientation;
494  Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
495  Ogre::Quaternion xquat = orientation * Ogre::Quaternion(quat.w, quat.x, quat.y, quat.z);
496  axes_vect[i]->setPosition(xpos);
497  axes_vect[i]->setOrientation(xquat);
498  }
499  break;
500 
501  case ARROWS:
502  allocateArrowVector(arrow_vect, num_points);
503  for (uint32_t i = 0; i < num_points; ++i)
504  {
505  const geometry_msgs::Point& pos = msg->poses[i].pose.position;
506  const geometry_msgs::Quaternion& quat = msg->poses[i].pose.orientation;
507  Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
508  Ogre::Quaternion xquat = orientation * Ogre::Quaternion(quat.w, quat.x, quat.y, quat.z);
509 
510  QColor color = pose_arrow_color_property_->getColor();
511  arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
512 
513  arrow_vect[i]->set(pose_arrow_shaft_length_property_->getFloat(),
517  arrow_vect[i]->setPosition(xpos);
518  arrow_vect[i]->setDirection(xquat * Ogre::Vector3(1, 0, 0));
519  }
520  break;
521 
522  default:
523  break;
524  }
526 }
527 
528 } // namespace rviz
529 
std::vector< Ogre::ManualObject * > manual_objects_
Definition: path_display.h:93
~PathDisplay() override
void setMin(float min)
std::vector< rviz::BillboardLine * > billboard_lines_
Definition: path_display.h:94
void addPoint(const Ogre::Vector3 &point)
FloatProperty * line_width_property_
Definition: path_display.h:101
ColorProperty * pose_arrow_color_property_
Definition: path_display.h:115
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
An object that displays a multi-segment line strip rendered as billboards.
virtual QColor getColor() const
EnumProperty * style_property_
Definition: path_display.h:98
FloatProperty * pose_arrow_shaft_diameter_property_
Definition: path_display.h:118
FloatProperty * pose_arrow_head_diameter_property_
Definition: path_display.h:119
FloatProperty * pose_arrow_shaft_length_property_
Definition: path_display.h:116
VectorProperty * offset_property_
Definition: path_display.h:103
void reset() override
Overridden from Display.
virtual Ogre::Vector3 getVector() const
Ogre::ColourValue getOgreColor() const
void setMin(int min)
FloatProperty * pose_axes_radius_property_
Definition: path_display.h:114
void allocateAxesVector(std::vector< rviz::Axes *> &axes_vect, int num)
Property specialized to enforce floating point max/min.
ColorProperty * color_property_
Definition: path_display.h:99
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:37
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
void processMessage(const nav_msgs::Path::ConstPtr &msg) override
Overridden from MessageFilterDisplay.
Displays a nav_msgs::Path message.
Definition: path_display.h:59
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
void onInitialize() override
Overridden from Display.
#define ROS_DEBUG_NAMED(name,...)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
void show()
Show this Property in any PropertyTreeWidgets.
Definition: property.h:410
std::vector< std::vector< rviz::Arrow * > > arrow_chain_
Definition: path_display.h:96
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:164
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void updatePoseArrowGeometry()
void allocateArrowVector(std::vector< rviz::Arrow *> &arrow_vect, int num)
void setNumLines(uint32_t num)
#define ROS_WARN_ONCE_NAMED(name,...)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Definition: axes.h:59
void setMaxPointsPerLine(uint32_t max)
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Definition: display.h:292
FloatProperty * alpha_property_
Definition: path_display.h:100
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:74
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
IntProperty * buffer_length_property_
Definition: path_display.h:102
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
virtual float getFloat() const
std::vector< std::vector< rviz::Axes * > > axes_chain_
Definition: path_display.h:95
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
EnumProperty * pose_style_property_
Definition: path_display.h:112
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:59
void destroyPoseArrowChain()
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:401
void updatePoseAxisGeometry()
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
Enum property.
Definition: enum_property.h:46
FloatProperty * pose_arrow_head_length_property_
Definition: path_display.h:117
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:175
FloatProperty * pose_axes_length_property_
Definition: path_display.h:113
void setLineWidth(float width)
#define ROS_DEBUG(...)


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