30 #ifndef COVARIANCE_VISUAL_H 31 #define COVARIANCE_VISUAL_H 37 #include <boost/scoped_ptr.hpp> 39 #include <geometry_msgs/PoseWithCovariance.h> 41 #include <Eigen/Dense> 43 #include <OgreVector3.h> 44 #include <OgreColourValue.h> 62 class CovarianceProperty;
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);
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 );
115 virtual void setPositionColor(
float r,
float g,
float b,
float a );
116 void setPositionColor(
const Ogre::ColourValue& color);
125 virtual void setOrientationColor(
float r,
float g,
float b,
float a );
126 void setOrientationColor(
const Ogre::ColourValue& color);
127 void setOrientationColorToRGB(
float a);
134 virtual void setCovariance(
const geometry_msgs::PoseWithCovariance& pose );
136 virtual const Ogre::Vector3& getPositionCovarianceScale();
137 virtual const Ogre::Quaternion& getPositionCovarianceOrientation();
166 virtual void setUserData(
const Ogre::Any& data );
173 virtual void setVisible(
bool visible );
178 virtual void setPositionVisible(
bool visible );
183 virtual void setOrientationVisible(
bool visible );
188 virtual void setPosition(
const Ogre::Vector3& position );
193 virtual void setOrientation(
const Ogre::Quaternion& orientation );
198 virtual void setRotatingFrame(
bool use_rotating_frame );
203 void updateOrientationVisibility();
211 Ogre::SceneNode* orientation_offset_node_[kNumOriShapes];
222 Ogre::Vector3 current_ori_scale_[kNumOriShapes];
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();
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.
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_
float current_ori_scale_factor_
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.