odometry_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 
31 #include "rviz/ogre_helpers/axes.h"
36 #include "rviz/validate_floats.h"
38 
39 #include <OgreSceneManager.h>
40 #include <OgreSceneNode.h>
41 
42 #include "odometry_display.h"
43 #include "covariance_property.h"
44 #include "covariance_visual.h"
45 
46 namespace rviz
47 {
48 
50 {
51 
52  position_tolerance_property_ = new FloatProperty( "Position Tolerance", .1,
53  "Distance, in meters from the last arrow dropped, "
54  "that will cause a new arrow to drop.",
55  this );
57 
58  angle_tolerance_property_ = new FloatProperty( "Angle Tolerance", .1,
59  "Angular distance from the last arrow dropped, "
60  "that will cause a new arrow to drop.",
61  this );
63 
64  keep_property_ = new IntProperty( "Keep", 100,
65  "Number of arrows to keep before removing the oldest. 0 means keep all of them.",
66  this );
67  keep_property_->setMin( 0 );
68 
69  shape_property_ = new EnumProperty( "Shape", "Arrow", "Shape to display the pose as.",
70  this, SLOT( updateShapeChoice() ));
73 
74  color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ),
75  "Color of the arrows.",
76  shape_property_, SLOT( updateColorAndAlpha() ), this);
77 
78  alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the arrow.",
79  shape_property_, SLOT( updateColorAndAlpha() ), this);
80  alpha_property_->setMin( 0 );
81  alpha_property_->setMax( 1 );
82 
83  shaft_length_property_ = new FloatProperty( "Shaft Length", 1, "Length of the each arrow's shaft, in meters.",
84  shape_property_, SLOT( updateArrowsGeometry() ), this);
85 
86  // aleeper: default changed from 0.1 to match change in arrow.cpp
87  shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.05, "Radius of the each arrow's shaft, in meters.",
88  shape_property_, SLOT( updateArrowsGeometry() ), this);
89 
90  head_length_property_ = new FloatProperty( "Head Length", 0.3, "Length of the each arrow's head, in meters.",
91  shape_property_, SLOT( updateArrowsGeometry() ), this);
92 
93  // aleeper: default changed from 0.2 to match change in arrow.cpp
94  head_radius_property_ = new FloatProperty( "Head Radius", 0.1, "Radius of the each arrow's head, in meters.",
95  shape_property_, SLOT( updateArrowsGeometry() ), this);
96 
97  axes_length_property_ = new FloatProperty( "Axes Length", 1, "Length of each axis, in meters.",
98  shape_property_, SLOT( updateAxisGeometry() ), this);
99 
100  axes_radius_property_ = new FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.",
101  shape_property_, SLOT( updateAxisGeometry() ), this);
102 
103  covariance_property_ = new CovarianceProperty( "Covariance", true, "Whether or not the covariances of the messages should be shown.",
104  this, SLOT( queueRender() ));
105 
106 }
107 
109 {
110  if ( initialized() )
111  {
112  clear();
113  }
114 }
115 
117 {
120 }
121 
123 {
126 }
127 
129 {
130  D_Arrow::iterator it = arrows_.begin();
131  D_Arrow::iterator end = arrows_.end();
132  for ( ; it != end; ++it )
133  {
134  delete *it;
135  }
136  arrows_.clear();
137 
138  // covariances are stored in covariance_property_
140 
141  D_Axes::iterator it_axes = axes_.begin();
142  D_Axes::iterator end_axes = axes_.end();
143  for ( ; it_axes != end_axes; ++it_axes )
144  {
145  delete *it_axes;
146  }
147  axes_.clear();
148 
149  if( last_used_message_ )
150  {
151  last_used_message_.reset();
152  }
153 }
154 
156 {
157  QColor color = color_property_->getColor();
158  float red = color.redF();
159  float green = color.greenF();
160  float blue = color.blueF();
161  float alpha = alpha_property_->getFloat();
162 
163  D_Arrow::iterator it = arrows_.begin();
164  D_Arrow::iterator end = arrows_.end();
165  for( ; it != end; ++it )
166  {
167  Arrow* arrow = *it;
168  arrow->setColor( red, green, blue, alpha );
169  }
171 }
172 
174 {
175  D_Arrow::iterator it = arrows_.begin();
176  D_Arrow::iterator end = arrows_.end();
177  for ( ; it != end; ++it )
178  {
179  updateGeometry(*it);
180  }
182 }
183 
185 {
186  D_Axes::iterator it = axes_.begin();
187  D_Axes::iterator end = axes_.end();
188  for ( ; it != end; ++it )
189  {
190  updateGeometry(*it);
191  }
193 }
194 
196 {
199 }
200 
202 {
207 }
208 
210 {
211  bool use_arrow = ( shape_property_->getOptionInt() == ArrowShape );
212 
213  color_property_->setHidden( !use_arrow );
214  alpha_property_->setHidden( !use_arrow );
215  shaft_length_property_->setHidden( !use_arrow );
216  shaft_radius_property_->setHidden( !use_arrow );
217  head_length_property_->setHidden( !use_arrow );
218  head_radius_property_->setHidden( !use_arrow );
219 
220  axes_length_property_->setHidden( use_arrow );
221  axes_radius_property_->setHidden( use_arrow );
222 
224 
226 }
227 
229 {
230  bool use_arrow = (shape_property_->getOptionInt() == ArrowShape);
231 
232  D_Arrow::iterator it = arrows_.begin();
233  D_Arrow::iterator end = arrows_.end();
234  for ( ; it != end; ++it )
235  {
236  (*it)->getSceneNode()->setVisible( use_arrow );
237  }
238 
239  D_Axes::iterator it_axes = axes_.begin();
240  D_Axes::iterator end_axes = axes_.end();
241  for ( ; it_axes != end_axes; ++it_axes )
242  {
243  (*it_axes)->getSceneNode()->setVisible( !use_arrow );
244  }
245 }
246 
247 bool validateFloats(const nav_msgs::Odometry& msg)
248 {
249  bool valid = true;
250  valid = valid && rviz::validateFloats( msg.pose.pose );
251  valid = valid && rviz::validateFloats( msg.pose.covariance );
252  valid = valid && rviz::validateFloats( msg.twist.twist );
253  // valid = valid && rviz::validateFloats( msg.twist.covariance )
254  return valid;
255 }
256 
257 void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& message )
258 {
259  typedef CovarianceProperty::CovarianceVisualPtr CovarianceVisualPtr;
260 
261  if( !validateFloats( *message ))
262  {
263  setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
264  return;
265  }
266 
267  if( !validateQuaternions( message->pose.pose ))
268  {
269  ROS_WARN_ONCE_NAMED( "quaternions", "Odometry '%s' contains unnormalized quaternions. "
270  "This warning will only be output once but may be true for others; "
271  "enable DEBUG messages for ros.rviz.quaternions to see more details.",
272  qPrintable( getName() ) );
273  ROS_DEBUG_NAMED( "quaternions", "Odometry '%s' contains unnormalized quaternions.",
274  qPrintable( getName() ) );
275  }
276 
277  if( last_used_message_ )
278  {
279  Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
280  Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
281  Eigen::Quaternionf last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
282  Eigen::Quaternionf current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
283 
284  if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
285  last_orientation.angularDistance(current_orientation) < angle_tolerance_property_->getFloat() )
286  {
287  return;
288  }
289  }
290 
291  Ogre::Vector3 position;
292  Ogre::Quaternion orientation;
293  if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation ))
294  {
295  ROS_ERROR( "Error transforming odometry '%s' from frame '%s' to frame '%s'",
296  qPrintable( getName() ), message->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
297  return;
298  }
299 
300  // If we arrive here, we're good. Continue...
301 
302  // Create a scene node, and attach the arrow and the covariance to it
303  Axes* axes = new Axes( scene_manager_, scene_node_,
306  Arrow* arrow = new Arrow( scene_manager_, scene_node_,
312 
313  // Position the axes
314  axes->setPosition( position );
315  axes->setOrientation( orientation );
316 
317  // Position the arrow. Remember the arrow points in -Z direction, so rotate the orientation before display.
318  arrow->setPosition( position );
319  arrow->setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ));
320 
321  // Position the frame where the covariance is attached covariance
322  cov->setPosition( position );
323  cov->setOrientation( orientation );
324 
325  // Set up arrow color
326  QColor color = color_property_->getColor();
327  float alpha = alpha_property_->getFloat();
328  arrow->setColor( color.redF(), color.greenF(), color.blueF(), alpha);
329 
330  // Set up the covariance based on the message
331  cov->setCovariance(message->pose);
332 
333  // Show/Hide things based on current properties
334  bool use_arrow = (shape_property_->getOptionInt() == ArrowShape);
335  arrow->getSceneNode()->setVisible( use_arrow );
336  axes->getSceneNode()->setVisible( !use_arrow );
337 
338  // store everything
339  axes_.push_back( axes );
340  arrows_.push_back( arrow );
341 
342  last_used_message_ = message;
344 }
345 
346 void OdometryDisplay::update( float wall_dt, float ros_dt )
347 {
348  size_t keep = keep_property_->getInt();
349  if( keep > 0 )
350  {
351  while( arrows_.size() > keep )
352  {
353  delete arrows_.front();
354  arrows_.pop_front();
355 
356  // covariance visuals are stored into covariance_property_
358 
359  delete axes_.front();
360  axes_.pop_front();
361  }
362  }
363 
364  assert(arrows_.size() == axes_.size());
365  assert(axes_.size() == covariance_property_->sizeVisual());
366 
367 }
368 
370 {
371  MFDClass::reset();
372  clear();
373 }
374 
375 } // namespace rviz
376 
virtual QColor getColor() const
void setMin(float min)
virtual void onInitialize()
Override this function to do subclass-specific initialization.
void setMax(float max)
nav_msgs::Odometry::ConstPtr last_used_message_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:73
virtual void processMessage(const nav_msgs::Odometry::ConstPtr &message)
rviz::FloatProperty * head_length_property_
virtual float getFloat() const
void setMin(int min)
rviz::FloatProperty * axes_radius_property_
Property specialized to enforce floating point max/min.
void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter)
Set the parameters for this arrow.
Definition: arrow.cpp:71
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
rviz::FloatProperty * position_tolerance_property_
CovarianceVisualPtr createAndPushBackVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
Definition: axes.cpp:84
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
rviz::FloatProperty * shaft_length_property_
#define ROS_DEBUG_NAMED(name,...)
rviz::EnumProperty * shape_property_
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
void queueRender()
Convenience function which calls context_->queueRender().
Definition: display.cpp:99
Accumulates and displays the pose from a nav_msgs::Odometry message.
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
rviz::FloatProperty * angle_tolerance_property_
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Transform a pose from a frame into the fixed frame.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void updateGeometry(rviz::Arrow *arrow)
CovarianceProperty * covariance_property_
#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
virtual void reset()
Called to tell the display to clear its state.
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.
rviz::IntProperty * keep_property_
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
Property specialized to provide getter for booleans.
rviz::FloatProperty * alpha_property_
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:59
void set(float length, float radius)
Set the parameters on this object.
Definition: axes.cpp:69
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Definition: axes.cpp:89
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
virtual void onEnable()
Overridden from MessageFilterDisplay to get Arrow/Axes visibility correct.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
rviz::FloatProperty * axes_length_property_
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:247
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Definition: axes.h:90
rviz::ColorProperty * color_property_
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
rviz::FloatProperty * head_radius_property_
#define ROS_ERROR(...)
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::FloatProperty * shaft_radius_property_


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