pose_array_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, 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 <OgreManualObject.h>
31 #include <OgreSceneManager.h>
32 #include <OgreSceneNode.h>
33 
34 #include "rviz/display_context.h"
35 #include "rviz/frame_manager.h"
39 #include "rviz/validate_floats.h"
42 #include "rviz/ogre_helpers/axes.h"
43 
45 
46 namespace rviz
47 {
48 
49 namespace
50 {
51  struct ShapeType
52  {
53  enum
54  {
55  Arrow2d,
56  Arrow3d,
57  Axes,
58  };
59  };
60 
61  Ogre::Vector3 vectorRosToOgre( geometry_msgs::Point const & point )
62  {
63  return Ogre::Vector3( point.x, point.y, point.z );
64  }
65 
66  Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion )
67  {
68  Ogre::Quaternion q;
69  normalizeQuaternion( quaternion, q );
70  return q;
71  }
72 }
73 
75  : manual_object_( NULL )
76 {
77  shape_property_ = new EnumProperty( "Shape", "Arrow (Flat)", "Shape to display the pose as.",
78  this, SLOT( updateShapeChoice() ) );
79 
80  arrow_color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.",
81  this, SLOT( updateArrowColor() ) );
82 
83  arrow_alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the displayed poses.",
84  this, SLOT( updateArrowColor() ) );
85 
86  arrow2d_length_property_ = new FloatProperty( "Arrow Length", 0.3, "Length of the arrows.",
87  this, SLOT( updateArrow2dGeometry() ) );
88 
89  arrow3d_head_radius_property_ = new FloatProperty( "Head Radius", 0.03, "Radius of the arrow's head, in meters.",
90  this, SLOT( updateArrow3dGeometry() ) );
91 
92  arrow3d_head_length_property_ = new FloatProperty( "Head Length", 0.07, "Length of the arrow's head, in meters.",
93  this, SLOT( updateArrow3dGeometry() ) );
94 
95  arrow3d_shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.01, "Radius of the arrow's shaft, in meters.",
96  this, SLOT( updateArrow3dGeometry() ) );
97 
98  arrow3d_shaft_length_property_ = new FloatProperty( "Shaft Length", 0.23, "Length of the arrow's shaft, in meters.",
99  this, SLOT( updateArrow3dGeometry() ) );
100 
101  axes_length_property_ = new FloatProperty( "Axes Length", 0.3, "Length of each axis, in meters.",
102  this, SLOT( updateAxesGeometry() ) );
103 
104  axes_radius_property_ = new FloatProperty( "Axes Radius", 0.01, "Radius of each axis, in meters.",
105  this, SLOT( updateAxesGeometry() ) );
106 
107  shape_property_->addOption( "Arrow (Flat)", ShapeType::Arrow2d );
108  shape_property_->addOption( "Arrow (3D)", ShapeType::Arrow3d );
109  shape_property_->addOption( "Axes", ShapeType::Axes );
112 }
113 
115 {
116  if ( initialized() )
117  {
118  scene_manager_->destroyManualObject( manual_object_ );
119  }
120 }
121 
123 {
125  manual_object_ = scene_manager_->createManualObject();
126  manual_object_->setDynamic( true );
127  scene_node_->attachObject( manual_object_ );
128  arrow_node_ = scene_node_->createChildSceneNode();
129  axes_node_ = scene_node_->createChildSceneNode();
131 }
132 
133 bool validateFloats( const geometry_msgs::PoseArray& msg )
134 {
135  return validateFloats( msg.poses );
136 }
137 
138 void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg )
139 {
140  if( !validateFloats( *msg ))
141  {
143  "Message contained invalid floating point values (nans or infs)" );
144  return;
145  }
146 
147  if( !validateQuaternions( msg->poses ))
148  {
149  ROS_WARN_ONCE_NAMED( "quaternions", "PoseArray msg received on topic '%s' contains unnormalized quaternions. "
150  "This warning will only be output once but may be true for others; "
151  "enable DEBUG messages for ros.rviz.quaternions to see more details.",
152  topic_property_->getTopicStd().c_str() );
153  ROS_DEBUG_NAMED( "quaternions", "PoseArray msg received on topic '%s' contains unnormalized quaternions.",
154  topic_property_->getTopicStd().c_str() );
155  }
156 
157  if( !setTransform( msg->header ) )
158  {
159  setStatus( StatusProperty::Error, "Topic", "Failed to look up transform" );
160  return;
161  }
162 
163  poses_.resize( msg->poses.size() );
164  for (std::size_t i = 0; i < msg->poses.size(); ++i)
165  {
166  poses_[i].position = vectorRosToOgre( msg->poses[i].position );
167  poses_[i].orientation = quaternionRosToOgre( msg->poses[i].orientation );
168  }
169 
170  updateDisplay();
172 }
173 
174 bool PoseArrayDisplay::setTransform( std_msgs::Header const & header )
175 {
176  Ogre::Vector3 position;
177  Ogre::Quaternion orientation;
178  if( !context_->getFrameManager()->getTransform( header, position, orientation ) )
179  {
180  ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
181  qPrintable( getName() ), header.frame_id.c_str(),
182  qPrintable( fixed_frame_ ) );
183  return false;
184  }
185  scene_node_->setPosition( position );
186  scene_node_->setOrientation( orientation );
187  return true;
188 }
189 
191 {
192  manual_object_->clear();
193 
194  Ogre::ColourValue color = arrow_color_property_->getOgreColor();
195  color.a = arrow_alpha_property_->getFloat();
197  size_t num_poses = poses_.size();
198  manual_object_->estimateVertexCount( num_poses * 6 );
199  manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
200  for( size_t i=0; i < num_poses; ++i )
201  {
202  const Ogre::Vector3 & pos = poses_[i].position;
203  const Ogre::Quaternion & orient = poses_[i].orientation;
204  Ogre::Vector3 vertices[6];
205  vertices[0] = pos; // back of arrow
206  vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow
207  vertices[2] = vertices[ 1 ];
208  vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 );
209  vertices[4] = vertices[ 1 ];
210  vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 );
211 
212  for( int i = 0; i < 6; ++i )
213  {
214  manual_object_->position( vertices[i] );
215  manual_object_->colour( color );
216  }
217  }
218  manual_object_->end();
219 }
220 
222 {
223  int shape = shape_property_->getOptionInt();
224  switch (shape) {
225  case ShapeType::Arrow2d:
226  updateArrows2d();
227  arrows3d_.clear();
228  axes_.clear();
229  break;
230  case ShapeType::Arrow3d:
231  updateArrows3d();
232  manual_object_->clear();
233  axes_.clear();
234  break;
235  case ShapeType::Axes:
236  updateAxes();
237  manual_object_->clear();
238  arrows3d_.clear();
239  break;
240  }
241 }
242 
244 {
245  while (arrows3d_.size() < poses_.size())
246  arrows3d_.push_back(makeArrow3d());
247  while (arrows3d_.size() > poses_.size())
248  arrows3d_.pop_back();
249 
250  Ogre::Quaternion adjust_orientation( Ogre::Degree(-90), Ogre::Vector3::UNIT_Y );
251  for (std::size_t i = 0; i < poses_.size(); ++i)
252  {
253  arrows3d_[i].setPosition( poses_[i].position );
254  arrows3d_[i].setOrientation( poses_[i].orientation * adjust_orientation );
255  }
256 }
257 
259 {
260  while (axes_.size() < poses_.size())
261  axes_.push_back(makeAxes());
262  while (axes_.size() > poses_.size())
263  axes_.pop_back();
264  for (std::size_t i = 0; i < poses_.size(); ++i)
265  {
266  axes_[i].setPosition( poses_[i].position );
267  axes_[i].setOrientation( poses_[i].orientation );
268  }
269 }
270 
272 {
273  Ogre::ColourValue color = arrow_color_property_->getOgreColor();
274  color.a = arrow_alpha_property_->getFloat();
275 
276  Arrow * arrow = new Arrow(
278  arrow_node_,
283  );
284 
285  arrow->setColor(color);
286  return arrow;
287 }
288 
290 {
291  return new Axes(
293  axes_node_,
296  );
297 }
298 
300 {
301  MFDClass::reset();
302  if( manual_object_ )
303  {
304  manual_object_->clear();
305  }
306  arrows3d_.clear();
307  axes_.clear();
308 }
309 
311 {
312  int shape = shape_property_->getOptionInt();
313  bool use_arrow2d = shape == ShapeType::Arrow2d;
314  bool use_arrow3d = shape == ShapeType::Arrow3d;
315  bool use_arrow = use_arrow2d || use_arrow3d;
316  bool use_axes = shape == ShapeType::Axes;
317 
318  arrow_color_property_->setHidden( !use_arrow );
319  arrow_alpha_property_->setHidden( !use_arrow );
320 
321  arrow2d_length_property_->setHidden(!use_arrow2d);
322 
323  arrow3d_shaft_length_property_->setHidden( !use_arrow3d );
324  arrow3d_shaft_radius_property_->setHidden( !use_arrow3d );
325  arrow3d_head_length_property_->setHidden( !use_arrow3d );
326  arrow3d_head_radius_property_->setHidden( !use_arrow3d );
327 
328  axes_length_property_->setHidden( !use_axes );
329  axes_radius_property_->setHidden( !use_axes );
330 
331  if (initialized())
332  updateDisplay();
333 }
334 
336 {
337  int shape = shape_property_->getOptionInt();
338  Ogre::ColourValue color = arrow_color_property_->getOgreColor();
339  color.a = arrow_alpha_property_->getFloat();
340 
341  if (shape == ShapeType::Arrow2d)
342  {
343  updateArrows2d();
344  }
345  else if (shape == ShapeType::Arrow3d)
346  {
347  for (std::size_t i = 0; i < arrows3d_.size(); ++i)
348  {
349  arrows3d_[i].setColor( color );
350  }
351  }
353 }
354 
356 {
357  updateArrows2d();
359 }
360 
362 {
363  for (std::size_t i = 0; i < poses_.size(); ++i)
364  {
365  arrows3d_[i].set(
370  );
371  }
373 }
374 
376 {
377  for (std::size_t i = 0; i < poses_.size(); ++i)
378  {
379  axes_[i].set(
382  );
383  }
385 }
386 
387 } // namespace rviz
388 
#define NULL
Definition: global.h:37
void setMin(float min)
FloatProperty * arrow3d_head_length_property_
void setMax(float max)
void updateAxesGeometry()
Update the axes geometry.
void updateShapeChoice()
Update the interface and visible shapes based on the selected shape type.
Ogre::ColourValue getOgreColor() const
ColorProperty * arrow_color_property_
FloatProperty * arrow2d_length_property_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
bool setTransform(std_msgs::Header const &header)
virtual float getFloat() const
Displays a geometry_msgs/PoseArray message as a bunch of line-drawn arrows.
RosTopicProperty * topic_property_
FloatProperty * axes_radius_property_
void updateArrow2dGeometry()
Update the flat arrow geometry.
Property specialized to enforce floating point max/min.
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
FloatProperty * arrow3d_shaft_radius_property_
#define ROS_DEBUG_NAMED(name,...)
std::vector< OgrePose > poses_
FloatProperty * arrow_alpha_property_
virtual void onInitialize()
Override this function to do subclass-specific initialization.
virtual void processMessage(const geometry_msgs::PoseArray::ConstPtr &msg)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
void updateArrowColor()
Update the arrow color.
virtual void setColor(float r, float g, float b, float a)
Set the color of this arrow. Sets both the head and shaft color to the same value. Values are in the range [0, 1].
Definition: arrow.cpp:86
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
FloatProperty * axes_length_property_
virtual void reset()
Called to tell the display to clear its state.
FloatProperty * arrow3d_shaft_length_property_
float normalizeQuaternion(float &w, float &x, float &y, float &z)
#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
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Definition: display.h:261
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:530
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
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 * shape_property_
void updateArrow3dGeometry()
Update the 3D arrow geometry.
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:59
boost::ptr_vector< Arrow > arrows3d_
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Ogre::SceneNode * arrow_node_
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:247
std::string getTopicStd() const
FloatProperty * arrow3d_head_radius_property_
boost::ptr_vector< Axes > axes_
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
#define ROS_ERROR(...)
Ogre::ManualObject * manual_object_
Ogre::SceneNode * axes_node_
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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51