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 {
49 {
50  position_tolerance_property_ = new FloatProperty("Position Tolerance", .1,
51  "Distance, in meters from the last arrow dropped, "
52  "that will cause a new arrow to drop.",
53  this);
55 
56  angle_tolerance_property_ = new FloatProperty("Angle Tolerance", .1,
57  "Angular distance from the last arrow dropped, "
58  "that will cause a new arrow to drop.",
59  this);
61 
63  new IntProperty("Keep", 100,
64  "Number of arrows to keep before removing the oldest. 0 means keep all of them.",
65  this);
67 
68  shape_property_ = new EnumProperty("Shape", "Arrow", "Shape to display the pose as.", this,
69  SLOT(updateShapeChoice()));
72 
73  color_property_ = new ColorProperty("Color", QColor(255, 25, 0), "Color of the arrows.",
74  shape_property_, SLOT(updateColorAndAlpha()), this);
75 
76  alpha_property_ = new FloatProperty("Alpha", 1, "Amount of transparency to apply to the arrow.",
77  shape_property_, SLOT(updateColorAndAlpha()), this);
80 
82  new FloatProperty("Shaft Length", 1, "Length of the each arrow's shaft, in meters.",
83  shape_property_, SLOT(updateArrowsGeometry()), this);
84 
85  // aleeper: default changed from 0.1 to match change in arrow.cpp
87  new FloatProperty("Shaft Radius", 0.05, "Radius of the each arrow's shaft, in meters.",
88  shape_property_, SLOT(updateArrowsGeometry()), this);
89 
91  new FloatProperty("Head Length", 0.3, "Length of the each arrow's head, in meters.",
92  shape_property_, SLOT(updateArrowsGeometry()), this);
93 
94  // aleeper: default changed from 0.2 to match change in arrow.cpp
96  new FloatProperty("Head Radius", 0.1, "Radius of the each arrow's head, in meters.",
97  shape_property_, SLOT(updateArrowsGeometry()), this);
98 
99  axes_length_property_ = new FloatProperty("Axes Length", 1, "Length of each axis, in meters.",
100  shape_property_, SLOT(updateAxisGeometry()), this);
101 
102  axes_radius_property_ = new FloatProperty("Axes Radius", 0.1, "Radius of each axis, in meters.",
103  shape_property_, SLOT(updateAxisGeometry()), this);
104 
106  new CovarianceProperty("Covariance", true,
107  "Whether or not the covariances of the messages should be shown.", this,
108  SLOT(queueRender()));
109 }
110 
112 {
113  if (initialized())
114  {
115  clear();
116  }
117 }
118 
120 {
123 }
124 
126 {
129 }
130 
132 {
133  D_Arrow::iterator it = arrows_.begin();
134  D_Arrow::iterator end = arrows_.end();
135  for (; it != end; ++it)
136  {
137  delete *it;
138  }
139  arrows_.clear();
140 
141  // covariances are stored in covariance_property_
143 
144  D_Axes::iterator it_axes = axes_.begin();
145  D_Axes::iterator end_axes = axes_.end();
146  for (; it_axes != end_axes; ++it_axes)
147  {
148  delete *it_axes;
149  }
150  axes_.clear();
151 
152  if (last_used_message_)
153  {
154  last_used_message_.reset();
155  }
156 }
157 
159 {
160  QColor color = color_property_->getColor();
161  float red = color.redF();
162  float green = color.greenF();
163  float blue = color.blueF();
164  float alpha = alpha_property_->getFloat();
165 
166  D_Arrow::iterator it = arrows_.begin();
167  D_Arrow::iterator end = arrows_.end();
168  for (; it != end; ++it)
169  {
170  Arrow* arrow = *it;
171  arrow->setColor(red, green, blue, alpha);
172  }
174 }
175 
177 {
178  D_Arrow::iterator it = arrows_.begin();
179  D_Arrow::iterator end = arrows_.end();
180  for (; it != end; ++it)
181  {
182  updateGeometry(*it);
183  }
185 }
186 
188 {
189  D_Axes::iterator it = axes_.begin();
190  D_Axes::iterator end = axes_.end();
191  for (; it != end; ++it)
192  {
193  updateGeometry(*it);
194  }
196 }
197 
199 {
201 }
202 
204 {
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  {
264  "Message contained invalid floating point values (nans or infs)");
265  return;
266  }
267 
268  if (!validateQuaternions(message->pose.pose))
269  {
270  ROS_WARN_ONCE_NAMED("quaternions",
271  "Odometry '%s' contains unnormalized quaternions. "
272  "This warning will only be output once but may be true for others; "
273  "enable DEBUG messages for ros.rviz.quaternions to see more details.",
274  qPrintable(getName()));
275  ROS_DEBUG_NAMED("quaternions", "Odometry '%s' contains unnormalized quaternions.",
276  qPrintable(getName()));
277  }
278 
279  if (last_used_message_)
280  {
281  Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x,
282  last_used_message_->pose.pose.position.y,
283  last_used_message_->pose.pose.position.z);
284  Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y,
285  message->pose.pose.position.z);
286  Eigen::Quaternionf last_orientation(last_used_message_->pose.pose.orientation.w,
287  last_used_message_->pose.pose.orientation.x,
288  last_used_message_->pose.pose.orientation.y,
289  last_used_message_->pose.pose.orientation.z);
290  Eigen::Quaternionf current_orientation(message->pose.pose.orientation.w,
291  message->pose.pose.orientation.x,
292  message->pose.pose.orientation.y,
293  message->pose.pose.orientation.z);
294 
295  if ((last_position - current_position).length() < position_tolerance_property_->getFloat() &&
296  last_orientation.angularDistance(current_orientation) < angle_tolerance_property_->getFloat())
297  {
298  return;
299  }
300  }
301 
302  Ogre::Vector3 position;
303  Ogre::Quaternion orientation;
304  if (!context_->getFrameManager()->transform(message->header, message->pose.pose, position, orientation))
305  {
306  ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
307  message->header.frame_id.c_str(), qPrintable(fixed_frame_));
308  return;
309  }
310 
311  // If we arrive here, we're good. Continue...
312 
313  // Create a scene node, and attach the arrow and the covariance to it
320 
321  // Position the axes
322  axes->setPosition(position);
323  axes->setOrientation(orientation);
324 
325  // Position the arrow. Remember the arrow points in -Z direction, so rotate the orientation before
326  // display.
327  arrow->setPosition(position);
328  arrow->setOrientation(orientation * Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y));
329 
330  // Position the frame where the covariance is attached covariance
331  cov->setPosition(position);
332  cov->setOrientation(orientation);
333 
334  // Set up arrow color
335  QColor color = color_property_->getColor();
336  float alpha = alpha_property_->getFloat();
337  arrow->setColor(color.redF(), color.greenF(), color.blueF(), alpha);
338 
339  // Set up the covariance based on the message
340  cov->setCovariance(message->pose);
341 
342  // Show/Hide things based on current properties
343  bool use_arrow = (shape_property_->getOptionInt() == ArrowShape);
344  arrow->getSceneNode()->setVisible(use_arrow);
345  axes->getSceneNode()->setVisible(!use_arrow);
346 
347  // store everything
348  axes_.push_back(axes);
349  arrows_.push_back(arrow);
350 
351  last_used_message_ = message;
353 }
354 
355 void OdometryDisplay::update(float /*wall_dt*/, float /*ros_dt*/)
356 {
357  size_t keep = keep_property_->getInt();
358  if (keep > 0)
359  {
360  while (arrows_.size() > keep)
361  {
362  delete arrows_.front();
363  arrows_.pop_front();
364 
365  // covariance visuals are stored into covariance_property_
367 
368  delete axes_.front();
369  axes_.pop_front();
370  }
371  }
372 
373  assert(arrows_.size() == axes_.size());
374  assert(axes_.size() == covariance_property_->sizeVisual());
375 }
376 
378 {
379  MFDClass::reset();
380  clear();
381 }
382 
383 } // namespace rviz
384 
void setMin(float min)
void setOrientation(const Ogre::Quaternion &orientation) override
Set the orientation of the object.
Definition: axes.cpp:88
void setMax(float max)
nav_msgs::Odometry::ConstPtr last_used_message_
void onInitialize() override
Override this function to do subclass-specific initialization.
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
virtual QColor getColor() const
void onEnable() override
Overridden from MessageFilterDisplay to get Arrow/Axes visibility correct.
rviz::FloatProperty * head_length_property_
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:74
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
rviz::FloatProperty * position_tolerance_property_
CovarianceVisualPtr createAndPushBackVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
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 reset() override
Called to tell the display to clear its state.
void queueRender()
Convenience function which calls context_->queueRender().
Definition: display.cpp:98
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:164
Accumulates and displays the pose from a nav_msgs::Odometry message.
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:59
void processMessage(const nav_msgs::Odometry::ConstPtr &message) override
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Definition: display.h:292
void setPosition(const Ogre::Vector3 &position) override
Set the position of this object.
Definition: axes.cpp:83
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:74
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:543
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)
virtual float getFloat() const
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:68
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void setColor(float r, float g, float b, float a) override
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:89
rviz::FloatProperty * axes_length_property_
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:275
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Definition: axes.h:95
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:46
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:175
rviz::FloatProperty * shaft_radius_property_


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Jan 9 2023 03:07:20