covariance_visual.h
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 #ifndef COVARIANCE_VISUAL_H
31 #define COVARIANCE_VISUAL_H
32 
33 #include <cmath>
34 
36 
37 #include <boost/scoped_ptr.hpp>
38 
39 #include <geometry_msgs/PoseWithCovariance.h>
40 
41 #include <Eigen/Dense>
42 
43 #include <OgreVector3.h>
44 #include <OgreColourValue.h>
45 
46 namespace Ogre
47 {
48 class SceneManager;
49 class SceneNode;
50 class Any;
51 }
52 
53 namespace Eigen
54 {
55  typedef Matrix<double,6,6> Matrix6d;
56 }
57 
58 namespace rviz
59 {
60 
61 class Shape;
62 class CovarianceProperty;
63 
69 {
70 public:
72  {
73  kRoll=0,
74  kPitch=1,
75  kYaw=2,
76  kYaw2D=3,
77  kNumOriShapes
78  };
79 
80 private:
93  CovarianceVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation, bool is_visible = true, float pos_scale = 1.0f, float ori_scale = 0.1f, float ori_offset = 0.1f);
94 public:
95  virtual ~CovarianceVisual();
96 
103  void setScales( float pos_scale, float ori_scale);
104  void setPositionScale( float pos_scale );
105  void setOrientationOffset( float ori_offset );
106  void setOrientationScale( float ori_scale );
107 
115  virtual void setPositionColor( float r, float g, float b, float a );
116  void setPositionColor(const Ogre::ColourValue& color);
117 
125  virtual void setOrientationColor( float r, float g, float b, float a );
126  void setOrientationColor(const Ogre::ColourValue& color);
127  void setOrientationColorToRGB(float a);
128 
134  virtual void setCovariance( const geometry_msgs::PoseWithCovariance& pose );
135 
136  virtual const Ogre::Vector3& getPositionCovarianceScale();
137  virtual const Ogre::Quaternion& getPositionCovarianceOrientation();
138 
143  Ogre::SceneNode* getPositionSceneNode() { return position_scale_node_; }
144 
149  Ogre::SceneNode* getOrientationSceneNode() { return orientation_root_node_; }
150 
155  rviz::Shape* getPositionShape() { return position_shape_; }
156 
161  rviz::Shape* getOrientationShape(ShapeIndex index);
162 
166  virtual void setUserData( const Ogre::Any& data );
167 
173  virtual void setVisible( bool visible );
174 
178  virtual void setPositionVisible( bool visible );
179 
183  virtual void setOrientationVisible( bool visible );
184 
188  virtual void setPosition( const Ogre::Vector3& position );
189 
193  virtual void setOrientation( const Ogre::Quaternion& orientation );
194 
198  virtual void setRotatingFrame( bool use_rotating_frame );
199 
200 private:
201  void updatePosition( const Eigen::Matrix6d& covariance );
202  void updateOrientation( const Eigen::Matrix6d& covariance, ShapeIndex index );
203  void updateOrientationVisibility();
204 
205  Ogre::SceneNode* root_node_;
206  Ogre::SceneNode* fixed_orientation_node_;
207  Ogre::SceneNode* position_scale_node_;
208  Ogre::SceneNode* position_node_;
209 
210  Ogre::SceneNode* orientation_root_node_;
211  Ogre::SceneNode* orientation_offset_node_[kNumOriShapes];
212 
214  rviz::Shape* orientation_shape_[kNumOriShapes];
215 
217 
218  bool pose_2d_;
219 
221 
222  Ogre::Vector3 current_ori_scale_[kNumOriShapes];
224 
225  const static float max_degrees;
226 
227 private:
228  // Hide Object methods we don't want to expose
229  // NOTE: Apparently we still need to define them...
230  virtual void setScale( const Ogre::Vector3& scale ) {};
231  virtual void setColor( float r, float g, float b, float a ) {};
232  virtual const Ogre::Vector3& getPosition();
233  virtual const Ogre::Quaternion& getOrientation();
234 
235  // Make CovarianceProperty friend class so it create CovarianceVisual objects
236  friend class CovarianceProperty;
237 };
238 
239 } // namespace rviz
240 
241 #endif /* COVARIANCE_VISUAL_H */
CovarianceVisual consisting in a ellipse for position and 2D ellipses along the axis for orientation...
Ogre::SceneNode * position_node_
virtual void setColor(float r, float g, float b, float a)
Set the color of the object. Values are in the range [0, 1].
Ogre::SceneNode * getPositionSceneNode()
Get the root scene node of the position part of this covariance.
Base class for visible objects, providing a minimal generic interface.
Definition: object.h:49
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
Ogre::SceneNode * orientation_root_node_
Matrix< double, 6, 6 > Matrix6d
Ogre::SceneNode * fixed_orientation_node_
bool orientation_visible_
If the orientation component is visible.
Ogre::SceneNode * position_scale_node_
rviz::Shape * getPositionShape()
Get the shape used to display position covariance.
Property specialized to provide getter for booleans.
rviz::Shape * position_shape_
Ellipse used for the position covariance.
static const float max_degrees
Ogre::SceneNode * root_node_
Ogre::SceneNode * getOrientationSceneNode()
Get the root scene node of the orientation part of this covariance.


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