covariance_property.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS
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 "covariance_property.h"
31 #include "covariance_visual.h"
32 
36 
37 #include <QColor>
38 
39 #include <OgreSceneManager.h>
40 #include <OgreSceneNode.h>
41 
42 namespace rviz
43 {
45  bool default_value,
46  const QString& description,
47  Property* parent)
48  // NOTE: changed() signal will only be initialized at the end of this constructor
49  : BoolProperty(name, default_value, description, parent)
50 {
52  new BoolProperty("Position", true, "Whether or not to show the position part of covariances", this,
55 
57  new ColorProperty("Color", QColor(204, 51, 204), "Color to draw the position covariance ellipse.",
60 
62  new FloatProperty("Alpha", 0.3f, "0 is fully transparent, 1.0 is fully opaque.",
67 
69  new FloatProperty("Scale", 1.0f,
70  "Scale factor to be applied to covariance ellipse. "
71  "Corresponds to the number of standard deviations to display",
75 
77  new BoolProperty("Orientation", true, "Whether or not to show the orientation part of covariances",
78  this, qOverload<>(&CovarianceProperty::updateVisibility));
80 
82  new EnumProperty("Frame", "Local", "The frame used to display the orientation covariance.",
84  this);
87 
89  "Color Style", "Unique",
90  "Style to color the orientation covariance: XYZ with same unique color or following RGB order",
94 
96  new ColorProperty("Color", QColor(255, 255, 127), "Color to draw the covariance ellipse.",
99 
101  new FloatProperty("Alpha", 0.5f, "0 is fully transparent, 1.0 is fully opaque.",
106 
108  "Offset", 1.0f,
109  "For 3D poses is the distance where to position the ellipses representing orientation covariance. "
110  "For 2D poses is the height of the triangle representing the variance on yaw.",
112  this);
114 
116  new FloatProperty("Scale", 1.0f,
117  "Scale factor to be applied to orientation covariance shapes. "
118  "Corresponds to the number of standard deviations to display.",
122 
124 
126 }
127 
129 {
130 }
131 
133 {
134  bool use_unique_color = (orientation_colorstyle_property_->getOptionInt() == Unique);
135  orientation_color_property_->setHidden(!use_unique_color);
137 }
138 
140 {
141  D_Covariance::iterator it_cov = covariances_.begin();
142  D_Covariance::iterator end_cov = covariances_.end();
143  for (; it_cov != end_cov; ++it_cov)
145 }
146 
148 {
149  float pos_alpha = position_alpha_property_->getFloat();
150  float pos_scale = position_scale_property_->getFloat();
151  QColor pos_color = position_color_property_->getColor();
152  visual->setPositionColor(pos_color.redF(), pos_color.greenF(), pos_color.blueF(), pos_alpha);
153  visual->setPositionScale(pos_scale);
154 
155  float ori_alpha = orientation_alpha_property_->getFloat();
156  float ori_offset = orientation_offset_property_->getFloat();
157  float ori_scale = orientation_scale_property_->getFloat();
159  {
160  QColor ori_color = orientation_color_property_->getColor();
161  visual->setOrientationColor(ori_color.redF(), ori_color.greenF(), ori_color.blueF(), ori_alpha);
162  }
163  else
164  {
165  visual->setOrientationColorToRGB(ori_alpha);
166  }
167  visual->setOrientationOffset(ori_offset);
168  visual->setOrientationScale(ori_scale);
169 }
170 
172 {
173  D_Covariance::iterator it_cov = covariances_.begin();
174  D_Covariance::iterator end_cov = covariances_.end();
175  for (; it_cov != end_cov; ++it_cov)
176  updateVisibility(*it_cov);
177 }
178 
180 {
181  bool show_covariance = getBool();
182  if (!show_covariance)
183  {
184  visual->setVisible(false);
185  }
186  else
187  {
188  bool show_position_covariance = position_property_->getBool();
189  ;
190  bool show_orientation_covariance = orientation_property_->getBool();
191  visual->setPositionVisible(show_position_covariance);
192  visual->setOrientationVisible(show_orientation_covariance);
193  }
194 }
195 
197 {
198  D_Covariance::iterator it_cov = covariances_.begin();
199  D_Covariance::iterator end_cov = covariances_.end();
200  for (; it_cov != end_cov; ++it_cov)
201  updateOrientationFrame(*it_cov);
202 }
203 
205 {
206  bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local);
207  visual->setRotatingFrame(use_rotating_frame);
208 }
209 
211 {
212  covariances_.pop_front();
213 }
214 
216 {
217  covariances_.clear();
218 }
219 
221 {
222  return covariances_.size();
223 }
224 
226 CovarianceProperty::createAndPushBackVisual(Ogre::SceneManager* scene_manager,
227  Ogre::SceneNode* parent_node)
228 {
229  bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local);
230  CovarianceVisualPtr visual(new CovarianceVisual(scene_manager, parent_node, use_rotating_frame));
231  updateVisibility(visual);
232  updateOrientationFrame(visual);
234  covariances_.push_back(visual);
235  return visual;
236 }
237 
239 {
240  return position_property_->getBool();
241 }
242 
244 {
245  return orientation_property_->getBool();
246 }
247 
248 } // end namespace rviz
rviz::CovarianceProperty::position_alpha_property_
rviz::FloatProperty * position_alpha_property_
Definition: covariance_property.h:133
rviz::BoolProperty::getBool
virtual bool getBool() const
Definition: bool_property.cpp:48
rviz::EnumProperty::getOptionInt
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
Definition: enum_property.cpp:56
rviz::CovarianceProperty::position_color_property_
rviz::ColorProperty * position_color_property_
Definition: covariance_property.h:132
rviz::ColorProperty::getColor
virtual QColor getColor() const
Definition: color_property.h:79
rviz::Property::setHidden
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:566
rviz::CovarianceProperty::Local
@ Local
Definition: covariance_property.h:64
boost::shared_ptr
rviz::CovarianceProperty::orientation_property_
rviz::BoolProperty * orientation_property_
Definition: covariance_property.h:135
rviz::CovarianceProperty::position_scale_property_
rviz::FloatProperty * position_scale_property_
Definition: covariance_property.h:134
rviz::CovarianceProperty::sizeVisual
size_t sizeVisual()
Definition: covariance_property.cpp:220
rviz::CovarianceProperty::createAndPushBackVisual
CovarianceVisualPtr createAndPushBackVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
Definition: covariance_property.cpp:226
rviz::CovarianceProperty::~CovarianceProperty
~CovarianceProperty() override
Definition: covariance_property.cpp:128
rviz::BoolProperty
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
rviz::FloatProperty::setMax
void setMax(float max)
Definition: float_property.cpp:57
description
description
rviz::CovarianceProperty::getPositionBool
bool getPositionBool()
Definition: covariance_property.cpp:238
rviz::CovarianceProperty::updateVisibility
void updateVisibility()
Definition: covariance_property.cpp:171
rviz::BoolProperty::setDisableChildrenIfFalse
void setDisableChildrenIfFalse(bool disable)
Definition: bool_property.cpp:53
float_property.h
rviz::CovarianceProperty::getOrientationBool
bool getOrientationBool()
Definition: covariance_property.cpp:243
rviz::CovarianceProperty::Unique
@ Unique
Definition: covariance_property.h:70
rviz::BoolProperty::BoolProperty
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=nullptr)
Definition: bool_property.cpp:36
rviz::FloatProperty::setMin
void setMin(float min)
Definition: float_property.cpp:51
f
f
rviz::ColorProperty
Definition: color_property.h:40
rviz::Property::connect
QMetaObject::Connection connect(const QObject *receiver, const char *slot, Qt::ConnectionType type=Qt::AutoConnection)
Connect changed() signal to given slot of receiver.
Definition: property.cpp:78
rviz::EnumProperty
Enum property.
Definition: enum_property.h:46
rviz::CovarianceProperty::updateColorStyleChoice
void updateColorStyleChoice()
Definition: covariance_property.cpp:132
rviz::FloatProperty
Property specialized to enforce floating point max/min.
Definition: float_property.h:37
rviz::Property
A single element of a property tree, with a name, value, description, and possibly children.
Definition: property.h:100
rviz::CovarianceProperty::clearVisual
void clearVisual()
Definition: covariance_property.cpp:215
covariance_property.h
rviz::CovarianceProperty::covariances_
D_Covariance covariances_
Definition: covariance_property.h:129
rviz::FloatProperty::getFloat
virtual float getFloat() const
Definition: float_property.h:79
rviz::EnumProperty::addOption
virtual void addOption(const QString &option, int value=0)
Definition: enum_property.cpp:50
rviz
Definition: add_display_dialog.cpp:54
rviz::CovarianceProperty::orientation_color_property_
rviz::ColorProperty * orientation_color_property_
Definition: covariance_property.h:138
color_property.h
rviz::CovarianceProperty::orientation_scale_property_
rviz::FloatProperty * orientation_scale_property_
Definition: covariance_property.h:141
rviz::CovarianceProperty::popFrontVisual
void popFrontVisual()
Definition: covariance_property.cpp:210
rviz::CovarianceProperty::updateColorAndAlphaAndScaleAndOffset
void updateColorAndAlphaAndScaleAndOffset()
Definition: covariance_property.cpp:139
rviz::CovarianceProperty::orientation_alpha_property_
rviz::FloatProperty * orientation_alpha_property_
Definition: covariance_property.h:139
rviz::CovarianceProperty::orientation_frame_property_
rviz::EnumProperty * orientation_frame_property_
Definition: covariance_property.h:136
covariance_visual.h
default_value
def default_value(type_)
rviz::CovarianceProperty::RGB
@ RGB
Definition: covariance_property.h:71
rviz::Property::changed
void changed()
Emitted by setValue() just after the value has changed.
rviz::CovarianceProperty::orientation_colorstyle_property_
rviz::EnumProperty * orientation_colorstyle_property_
Definition: covariance_property.h:137
rviz::CovarianceVisual
CovarianceVisual consisting in a ellipse for position and 2D ellipses along the axis for orientation.
Definition: covariance_visual.h:68
rviz::CovarianceProperty::Fixed
@ Fixed
Definition: covariance_property.h:65
rviz::CovarianceProperty::orientation_offset_property_
rviz::FloatProperty * orientation_offset_property_
Definition: covariance_property.h:140
rviz::CovarianceProperty::position_property_
rviz::BoolProperty * position_property_
Definition: covariance_property.h:131
rviz::CovarianceProperty::updateOrientationFrame
void updateOrientationFrame()
Definition: covariance_property.cpp:196
rviz::CovarianceProperty::CovarianceProperty
CovarianceProperty(const QString &name="Covariance", bool default_value=false, const QString &description=QString(), rviz::Property *parent=nullptr)
Definition: covariance_property.cpp:44
enum_property.h


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:09