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 <OgreBillboardSet.h>
36 #include <OgreMatrix4.h>
37 
38 #include <tf/transform_listener.h>
39 
40 #include "rviz/display_context.h"
41 #include "rviz/frame_manager.h"
47 #include "rviz/validate_floats.h"
49 
52 
53 namespace rviz
54 {
55 
57 {
58  style_property_ = new EnumProperty( "Line Style", "Lines",
59  "The rendering operation to use to draw the grid lines.",
60  this, SLOT( updateStyle() ));
61 
62  style_property_->addOption( "Lines", LINES );
63  style_property_->addOption( "Billboards", BILLBOARDS );
64 
65  line_width_property_ = new FloatProperty( "Line Width", 0.03,
66  "The width, in meters, of each path line."
67  "Only works with the 'Billboards' style.",
68  this, SLOT( updateLineWidth() ), this );
69  line_width_property_->setMin( 0.001 );
71 
72  color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
73  "Color to draw the path.", this );
74 
75  alpha_property_ = new FloatProperty( "Alpha", 1.0,
76  "Amount of transparency to apply to the path.", this );
77 
78  buffer_length_property_ = new IntProperty( "Buffer Length", 1,
79  "Number of paths to display.",
80  this, SLOT( updateBufferLength() ));
82 
83  offset_property_ = new VectorProperty( "Offset", Ogre::Vector3::ZERO,
84  "Allows you to offset the path from the origin of the reference frame. In meters.",
85  this, SLOT( updateOffset() ));
86 
87  pose_style_property_ = new EnumProperty( "Pose Style", "None", "Shape to display the pose as.",
88  this, SLOT( updatePoseStyle() ));
92 
94  "Length of the axes.",
95  this, SLOT(updatePoseAxisGeometry()) );
96  pose_axes_radius_property_ = new rviz::FloatProperty( "Radius", 0.03,
97  "Radius of the axes.",
98  this, SLOT(updatePoseAxisGeometry()) );
99 
100  pose_arrow_color_property_ = new ColorProperty( "Pose Color",
101  QColor( 255, 85, 255 ),
102  "Color to draw the poses.",
103  this, SLOT(updatePoseArrowColor()));
104  pose_arrow_shaft_length_property_ = new rviz::FloatProperty( "Shaft Length", 0.1,
105  "Length of the arrow shaft.",
106  this,
107  SLOT(updatePoseArrowGeometry()));
108  pose_arrow_head_length_property_ = new rviz::FloatProperty( "Head Length", 0.2,
109  "Length of the arrow head.",
110  this,
111  SLOT(updatePoseArrowGeometry()));
112  pose_arrow_shaft_diameter_property_ = new rviz::FloatProperty( "Shaft Diameter", 0.1,
113  "Diameter of the arrow shaft.",
114  this,
115  SLOT(updatePoseArrowGeometry()));
116  pose_arrow_head_diameter_property_ = new rviz::FloatProperty( "Head Diameter", 0.3,
117  "Diameter of the arrow head.",
118  this,
119  SLOT(updatePoseArrowGeometry()));
127 }
128 
130 {
131  destroyObjects();
134 }
135 
137 {
140 }
141 
143 {
144  MFDClass::reset();
146 }
147 
148 
149 void PathDisplay::allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, int num)
150 {
151  if (num > axes_vect.size()) {
152  for (size_t i = axes_vect.size(); i < num; i++) {
156  axes_vect.push_back(axes);
157  }
158  }
159  else if (num < axes_vect.size()) {
160  for (int i = axes_vect.size() - 1; num <= i; i--) {
161  delete axes_vect[i];
162  }
163  axes_vect.resize(num);
164  }
165 }
166 
167 void PathDisplay::allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, int num)
168 {
169  if (num > arrow_vect.size()) {
170  for (size_t i = arrow_vect.size(); i < num; i++) {
172  arrow_vect.push_back(arrow);
173  }
174  }
175  else if (num < arrow_vect.size()) {
176  for (int i = arrow_vect.size() - 1; num <= i; i--) {
177  delete arrow_vect[i];
178  }
179  arrow_vect.resize(num);
180  }
181 }
182 
184 {
185  for( size_t i = 0; i < axes_chain_.size(); i++ )
186  {
188  }
189  axes_chain_.resize(0);
190 }
191 
193 {
194  for( size_t i = 0; i < arrow_chain_.size(); i++ )
195  {
197  }
198  arrow_chain_.resize(0);
199 }
200 
202 {
204 
205  switch( style )
206  {
207  case LINES:
208  default:
210  break;
211 
212  case BILLBOARDS:
214  break;
215  }
216 
218 }
219 
221 {
223  float line_width = line_width_property_->getFloat();
224 
225  if(style == BILLBOARDS) {
226  for( size_t i = 0; i < billboard_lines_.size(); i++ )
227  {
228  rviz::BillboardLine* billboard_line = billboard_lines_[ i ];
229  if( billboard_line ) billboard_line->setLineWidth( line_width );
230  }
231  }
233 }
234 
236 {
237  scene_node_->setPosition( offset_property_->getVector() );
239 }
240 
242 {
244  switch (pose_style)
245  {
246  case AXES:
254  break;
255  case ARROWS:
263  break;
264  default:
272  }
274 }
275 
277 {
278  for(size_t i = 0; i < axes_chain_.size() ; i++)
279  {
280  std::vector<rviz::Axes*>& axes_vect = axes_chain_[i];
281  for(size_t j = 0; j < axes_vect.size() ; j++)
282  {
283  axes_vect[j]->set( pose_axes_length_property_->getFloat(),
285  }
286  }
288 }
289 
291 {
292  QColor color = pose_arrow_color_property_->getColor();
293 
294  for( size_t i = 0; i < arrow_chain_.size(); i++ )
295  {
296  std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
297  for( size_t j = 0; j < arrow_vect.size(); j++ )
298  {
299  arrow_vect[j]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
300  }
301  }
303 }
304 
306 {
307  for( size_t i = 0; i < arrow_chain_.size(); i++ )
308  {
309  std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
310  for( size_t j = 0; j < arrow_vect.size(); j++ )
311  {
312  arrow_vect[j]->set(pose_arrow_shaft_length_property_->getFloat(),
316  }
317  }
319 }
320 
322 {
323  // Destroy all simple lines, if any
324  for( size_t i = 0; i < manual_objects_.size(); i++ )
325  {
326  Ogre::ManualObject*& manual_object = manual_objects_[ i ];
327  if( manual_object )
328  {
329  manual_object->clear();
330  scene_manager_->destroyManualObject( manual_object );
331  manual_object = NULL; // ensure it doesn't get destroyed again
332  }
333  }
334 
335  // Destroy all billboards, if any
336  for( size_t i = 0; i < billboard_lines_.size(); i++ )
337  {
338  rviz::BillboardLine*& billboard_line = billboard_lines_[ i ];
339  if( billboard_line )
340  {
341  delete billboard_line; // also destroys the corresponding scene node
342  billboard_line = NULL; // ensure it doesn't get destroyed again
343  }
344  }
345 }
346 
348 {
349  // Delete old path objects
350  destroyObjects();
351 
352  // Destroy all axes and arrows
355 
356  // Read options
357  int buffer_length = buffer_length_property_->getInt();
359 
360  // Create new path objects
361  switch(style)
362  {
363  case LINES: // simple lines with fixed width of 1px
364  manual_objects_.resize( buffer_length );
365  for( size_t i = 0; i < manual_objects_.size(); i++ )
366  {
367  Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
368  manual_object->setDynamic( true );
369  scene_node_->attachObject( manual_object );
370 
371  manual_objects_[ i ] = manual_object;
372  }
373  break;
374 
375  case BILLBOARDS: // billboards with configurable width
376  billboard_lines_.resize( buffer_length );
377  for( size_t i = 0; i < billboard_lines_.size(); i++ )
378  {
380  billboard_lines_[ i ] = billboard_line;
381  }
382  break;
383  }
384  axes_chain_.resize( buffer_length );
385  arrow_chain_.resize( buffer_length );
386 
387 
388 }
389 
390 bool validateFloats( const nav_msgs::Path& msg )
391 {
392  bool valid = true;
393  valid = valid && validateFloats( msg.poses );
394  return valid;
395 }
396 
397 void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
398 {
399  // Calculate index of oldest element in cyclic buffer
400  size_t bufferIndex = messages_received_ % buffer_length_property_->getInt();
401 
403  Ogre::ManualObject* manual_object = NULL;
404  rviz::BillboardLine* billboard_line = NULL;
405 
406  // Delete oldest element
407  switch(style)
408  {
409  case LINES:
410  manual_object = manual_objects_[ bufferIndex ];
411  manual_object->clear();
412  break;
413 
414  case BILLBOARDS:
415  billboard_line = billboard_lines_[ bufferIndex ];
416  billboard_line->clear();
417  break;
418  }
419 
420  // Check if path contains invalid coordinate values
421  if( !validateFloats( *msg ))
422  {
423  setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
424  return;
425  }
426 
427  if( !validateQuaternions( msg->poses ))
428  {
429  ROS_WARN_ONCE_NAMED( "quaternions", "Path '%s' contains unnormalized quaternions. "
430  "This warning will only be output once but may be true for others; "
431  "enable DEBUG messages for ros.rviz.quaternions to see more details.",
432  qPrintable( getName() ) );
433  ROS_DEBUG_NAMED( "quaternions", "Path '%s' contains unnormalized quaternions.",
434  qPrintable( getName() ) );
435  }
436 
437  // Lookup transform into fixed frame
438  Ogre::Vector3 position;
439  Ogre::Quaternion orientation;
440  if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
441  {
442  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
443  }
444 
445  Ogre::Matrix4 transform( orientation );
446  transform.setTrans( position );
447 
448 // scene_node_->setPosition( position );
449 // scene_node_->setOrientation( orientation );
450 
451  Ogre::ColourValue color = color_property_->getOgreColor();
452  color.a = alpha_property_->getFloat();
453 
454  uint32_t num_points = msg->poses.size();
455  float line_width = line_width_property_->getFloat();
456 
457  switch(style)
458  {
459  case LINES:
460  manual_object->estimateVertexCount( num_points );
461  manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
462  for( uint32_t i=0; i < num_points; ++i)
463  {
464  const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
465  Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
466  manual_object->position( xpos.x, xpos.y, xpos.z );
467  manual_object->colour( color );
468  }
469 
470  manual_object->end();
471  break;
472 
473  case BILLBOARDS:
474  billboard_line->setNumLines( 1 );
475  billboard_line->setMaxPointsPerLine( num_points );
476  billboard_line->setLineWidth( line_width );
477 
478  for( uint32_t i=0; i < num_points; ++i)
479  {
480  const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
481  Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
482  billboard_line->addPoint( xpos, color );
483  }
484 
485  break;
486  }
487 
488  // process pose markers
490  std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[ bufferIndex ];
491  std::vector<rviz::Axes*>& axes_vect = axes_chain_[ bufferIndex ];
492 
493  switch(pose_style)
494  {
495  case AXES:
496  allocateAxesVector(axes_vect, num_points);
497  for( uint32_t i=0; i < num_points; ++i)
498  {
499  const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
500  const geometry_msgs::Quaternion& quat = msg->poses[ i ].pose.orientation;
501  Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
502  Ogre::Quaternion xquat = orientation * Ogre::Quaternion( quat.w, quat.x, quat.y, quat.z );
503  axes_vect[i]->setPosition(xpos);
504  axes_vect[i]->setOrientation(xquat);
505  }
506  break;
507 
508  case ARROWS:
509  allocateArrowVector(arrow_vect, num_points);
510  for( uint32_t i=0; i < num_points; ++i)
511  {
512  const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
513  const geometry_msgs::Quaternion& quat = msg->poses[ i ].pose.orientation;
514  Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
515  Ogre::Quaternion xquat = orientation * Ogre::Quaternion( quat.w, quat.x, quat.y, quat.z );
516 
517  QColor color = pose_arrow_color_property_->getColor();
518  arrow_vect[i]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
519 
520  arrow_vect[i]->set(pose_arrow_shaft_length_property_->getFloat(),
524  arrow_vect[i]->setPosition(xpos);
525  arrow_vect[i]->setDirection(xquat * Ogre::Vector3(1, 0, 0));
526  }
527  break;
528 
529  default:
530  break;
531  }
533 }
534 
535 } // namespace rviz
536 
virtual QColor getColor() const
std::vector< Ogre::ManualObject * > manual_objects_
Definition: path_display.h:94
#define NULL
Definition: global.h:37
void setMin(float min)
Ogre::ColourValue getOgreColor() const
std::vector< rviz::BillboardLine * > billboard_lines_
Definition: path_display.h:95
void addPoint(const Ogre::Vector3 &point)
FloatProperty * line_width_property_
Definition: path_display.h:102
ColorProperty * pose_arrow_color_property_
Definition: path_display.h:115
void allocateAxesVector(std::vector< rviz::Axes * > &axes_vect, int num)
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
An object that displays a multi-segment line strip rendered as billboards.
EnumProperty * style_property_
Definition: path_display.h:99
FloatProperty * pose_arrow_shaft_diameter_property_
Definition: path_display.h:118
FloatProperty * pose_arrow_head_diameter_property_
Definition: path_display.h:119
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:73
FloatProperty * pose_arrow_shaft_length_property_
Definition: path_display.h:116
virtual float getFloat() const
VectorProperty * offset_property_
Definition: path_display.h:104
void setMin(int min)
FloatProperty * pose_axes_radius_property_
Definition: path_display.h:114
Property specialized to enforce floating point max/min.
ColorProperty * color_property_
Definition: path_display.h:100
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:38
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
Displays a nav_msgs::Path message.
Definition: path_display.h:60
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
#define ROS_DEBUG_NAMED(name,...)
virtual void reset()
Overridden from Display.
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:377
std::vector< std::vector< rviz::Arrow * > > arrow_chain_
Definition: path_display.h:97
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void updatePoseArrowGeometry()
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:58
void allocateArrowVector(std::vector< rviz::Arrow * > &arrow_vect, int num)
void setMaxPointsPerLine(uint32_t max)
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Definition: display.h:261
FloatProperty * alpha_property_
Definition: path_display.h:101
void processMessage(const nav_msgs::Path::ConstPtr &msg)
Overridden from MessageFilterDisplay.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
virtual Ogre::Vector3 getVector() const
IntProperty * buffer_length_property_
Definition: path_display.h:103
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
std::vector< std::vector< rviz::Axes * > > axes_chain_
Definition: path_display.h:96
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:371
void updatePoseAxisGeometry()
virtual ~PathDisplay()
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:47
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:186
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:159
FloatProperty * pose_axes_length_property_
Definition: path_display.h:113
void setLineWidth(float width)
virtual void onInitialize()
Overridden from Display.
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41