From 73118980f3ae2caee359802ae2c1427bad9db3fc Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Wed, 3 May 2017 14:50:43 +0200 Subject: [PATCH 1/6] Include covariance_visual.h as local file --- src/rviz/default_plugin/covariance_property.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/default_plugin/covariance_property.cpp b/src/rviz/default_plugin/covariance_property.cpp index ee9b77135c..5586822f17 100644 --- a/src/rviz/default_plugin/covariance_property.cpp +++ b/src/rviz/default_plugin/covariance_property.cpp @@ -28,8 +28,8 @@ */ #include "covariance_property.h" +#include "covariance_visual.h" -#include "rviz/default_plugin/covariance_visual.h" #include "rviz/properties/color_property.h" #include "rviz/properties/float_property.h" #include "rviz/properties/enum_property.h" From bd6a01c9531e55e0147d2f50502c2772562e3271 Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Wed, 3 May 2017 14:54:58 +0200 Subject: [PATCH 2/6] Use rviz namespace explicitely; Clean trailing whitespaces The use of rviz:: make the files close to the ones of the original plugin --- src/rviz/default_plugin/covariance_property.h | 24 +++++------ src/rviz/default_plugin/odometry_display.cpp | 16 ++++---- src/rviz/default_plugin/odometry_display.h | 40 +++++++++---------- .../pose_with_covariance_display.cpp | 12 +++--- .../pose_with_covariance_display.h | 25 ++++++------ 5 files changed, 59 insertions(+), 58 deletions(-) diff --git a/src/rviz/default_plugin/covariance_property.h b/src/rviz/default_plugin/covariance_property.h index f6147ab973..0461ead95c 100644 --- a/src/rviz/default_plugin/covariance_property.h +++ b/src/rviz/default_plugin/covariance_property.h @@ -52,7 +52,7 @@ class EnumProperty; class CovarianceVisual; /** @brief Property specialized to provide getter for booleans. */ -class CovarianceProperty: public BoolProperty +class CovarianceProperty: public rviz::BoolProperty { Q_OBJECT public: @@ -73,7 +73,7 @@ Q_OBJECT CovarianceProperty( const QString& name = "Covariance", bool default_value = false, const QString& description = QString(), - Property* parent = 0, + rviz::Property* parent = 0, const char *changed_slot = 0, QObject* receiver = 0 ); @@ -104,16 +104,16 @@ private Q_SLOTS: typedef std::deque D_Covariance; D_Covariance covariances_; - BoolProperty* position_property_; - ColorProperty* position_color_property_; - FloatProperty* position_alpha_property_; - FloatProperty* position_scale_property_; - BoolProperty* orientation_property_; - EnumProperty* orientation_frame_property_; - EnumProperty* orientation_colorstyle_property_; - ColorProperty* orientation_color_property_; - FloatProperty* orientation_alpha_property_; - FloatProperty* orientation_scale_property_; + rviz::BoolProperty* position_property_; + rviz::ColorProperty* position_color_property_; + rviz::FloatProperty* position_alpha_property_; + rviz::FloatProperty* position_scale_property_; + rviz::BoolProperty* orientation_property_; + rviz::EnumProperty* orientation_frame_property_; + rviz::EnumProperty* orientation_colorstyle_property_; + rviz::ColorProperty* orientation_color_property_; + rviz::FloatProperty* orientation_alpha_property_; + rviz::FloatProperty* orientation_scale_property_; }; } // end namespace rviz diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index ce7f388c1f..6518e7f240 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/src/rviz/default_plugin/odometry_display.cpp @@ -53,7 +53,7 @@ OdometryDisplay::OdometryDisplay() "that will cause a new arrow to drop.", this ); position_tolerance_property_->setMin( 0 ); - + angle_tolerance_property_ = new FloatProperty( "Angle Tolerance", .1, "Angular distance from the last arrow dropped, " "that will cause a new arrow to drop.", @@ -85,7 +85,7 @@ OdometryDisplay::OdometryDisplay() // aleeper: default changed from 0.1 to match change in arrow.cpp shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.05, "Radius of the each arrow's shaft, in meters.", shape_property_, SLOT( updateArrowsGeometry() ), this); - + head_length_property_ = new FloatProperty( "Head Length", 0.3, "Length of the each arrow's head, in meters.", shape_property_, SLOT( updateArrowsGeometry() ), this); @@ -101,7 +101,7 @@ OdometryDisplay::OdometryDisplay() covariance_property_ = new CovarianceProperty( "Covariance", true, "Whether or not the covariances of the messages should be shown.", this, SLOT( queueRender() )); - + } OdometryDisplay::~OdometryDisplay() @@ -246,10 +246,10 @@ void OdometryDisplay::updateShapeVisibility() bool validateFloats(const nav_msgs::Odometry& msg) { bool valid = true; - valid = valid && validateFloats( msg.pose.pose ); - valid = valid && validateFloats( msg.pose.covariance ); - valid = valid && validateFloats( msg.twist.twist ); - // valid = valid && validateFloats( msg.twist.covariance ) + valid = valid && rviz::validateFloats( msg.pose.pose ); + valid = valid && rviz::validateFloats( msg.pose.covariance ); + valid = valid && rviz::validateFloats( msg.twist.twist ); + // valid = valid && rviz::validateFloats( msg.twist.covariance ) return valid; } @@ -293,7 +293,7 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag Axes* axes = new Axes( scene_manager_, scene_node_, axes_length_property_->getFloat(), axes_radius_property_->getFloat() ); - Arrow* arrow = new Arrow( scene_manager_, scene_node_, + Arrow* arrow = new Arrow( scene_manager_, scene_node_, shaft_length_property_->getFloat(), shaft_radius_property_->getFloat(), head_length_property_->getFloat(), diff --git a/src/rviz/default_plugin/odometry_display.h b/src/rviz/default_plugin/odometry_display.h index f7a18d4ed5..dc7629636d 100644 --- a/src/rviz/default_plugin/odometry_display.h +++ b/src/rviz/default_plugin/odometry_display.h @@ -59,7 +59,7 @@ class CovarianceProperty; * \class OdometryDisplay * \brief Accumulates and displays the pose from a nav_msgs::Odometry message */ -class OdometryDisplay: public MessageFilterDisplay +class OdometryDisplay: public rviz::MessageFilterDisplay { Q_OBJECT public: @@ -90,35 +90,35 @@ private Q_SLOTS: void updateAxisGeometry(); private: - void updateGeometry( Arrow* arrow ); - void updateGeometry( Axes* axes ); + void updateGeometry( rviz::Arrow* arrow ); + void updateGeometry( rviz::Axes* axes ); void clear(); virtual void processMessage( const nav_msgs::Odometry::ConstPtr& message ); - typedef std::deque D_Arrow; - typedef std::deque D_Axes; + typedef std::deque D_Arrow; + typedef std::deque D_Axes; D_Arrow arrows_; D_Axes axes_; nav_msgs::Odometry::ConstPtr last_used_message_; - EnumProperty* shape_property_; - - ColorProperty* color_property_; - FloatProperty* alpha_property_; - FloatProperty* position_tolerance_property_; - FloatProperty* angle_tolerance_property_; - IntProperty* keep_property_; - - FloatProperty* head_radius_property_; - FloatProperty* head_length_property_; - FloatProperty* shaft_radius_property_; - FloatProperty* shaft_length_property_; - - FloatProperty* axes_length_property_; - FloatProperty* axes_radius_property_; + rviz::EnumProperty* shape_property_; + + rviz::ColorProperty* color_property_; + rviz::FloatProperty* alpha_property_; + rviz::FloatProperty* position_tolerance_property_; + rviz::FloatProperty* angle_tolerance_property_; + rviz::IntProperty* keep_property_; + + rviz::FloatProperty* head_radius_property_; + rviz::FloatProperty* head_length_property_; + rviz::FloatProperty* shaft_radius_property_; + rviz::FloatProperty* shaft_length_property_; + + rviz::FloatProperty* axes_length_property_; + rviz::FloatProperty* axes_radius_property_; CovarianceProperty* covariance_property_; }; diff --git a/src/rviz/default_plugin/pose_with_covariance_display.cpp b/src/rviz/default_plugin/pose_with_covariance_display.cpp index cdb84f1650..361aa20f15 100644 --- a/src/rviz/default_plugin/pose_with_covariance_display.cpp +++ b/src/rviz/default_plugin/pose_with_covariance_display.cpp @@ -46,8 +46,8 @@ #include "rviz/validate_floats.h" #include "pose_with_covariance_display.h" -#include "covariance_property.h" #include "covariance_visual.h" +#include "covariance_property.h" #include @@ -80,7 +80,7 @@ class PoseWithCovarianceDisplaySelectionHandler: public SelectionHandler covariance_position_property_->setReadOnly( true ); covariance_orientation_property_ = new VectorProperty( "Covariance Orientation", Ogre::Vector3::ZERO, "", cat ); - covariance_orientation_property_->setReadOnly( true ); + covariance_orientation_property_->setReadOnly( true ); } void getAABBs( const Picked& obj, V_AABB& aabbs ) @@ -148,7 +148,7 @@ class PoseWithCovarianceDisplaySelectionHandler: public SelectionHandler QuaternionProperty* orientation_property_; VectorProperty* covariance_position_property_; VectorProperty* covariance_orientation_property_; - + }; PoseWithCovarianceDisplay::PoseWithCovarianceDisplay() @@ -173,7 +173,7 @@ PoseWithCovarianceDisplay::PoseWithCovarianceDisplay() // aleeper: default changed from 0.1 to match change in arrow.cpp shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.05, "Radius of the arrow's shaft, in meters.", this, SLOT( updateArrowGeometry() )); - + head_length_property_ = new FloatProperty( "Head Length", 0.3, "Length of the arrow's head, in meters.", this, SLOT( updateArrowGeometry() )); @@ -195,7 +195,7 @@ void PoseWithCovarianceDisplay::onInitialize() { MFDClass::onInitialize(); - arrow_ = new Arrow( scene_manager_, scene_node_, + arrow_ = new rviz::Arrow( scene_manager_, scene_node_, shaft_length_property_->getFloat(), shaft_radius_property_->getFloat(), head_length_property_->getFloat(), @@ -204,7 +204,7 @@ void PoseWithCovarianceDisplay::onInitialize() // TODO: is it safe to change Arrow to point in +X direction? arrow_->setOrientation( Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y )); - axes_ = new Axes( scene_manager_, scene_node_, + axes_ = new rviz::Axes( scene_manager_, scene_node_, axes_length_property_->getFloat(), axes_radius_property_->getFloat() ); diff --git a/src/rviz/default_plugin/pose_with_covariance_display.h b/src/rviz/default_plugin/pose_with_covariance_display.h index 4d9904c563..e1e7c105dc 100644 --- a/src/rviz/default_plugin/pose_with_covariance_display.h +++ b/src/rviz/default_plugin/pose_with_covariance_display.h @@ -39,6 +39,7 @@ namespace rviz { + class Arrow; class Axes; class ColorProperty; @@ -54,7 +55,7 @@ class PoseWithCovarianceDisplaySelectionHandler; typedef boost::shared_ptr PoseWithCovarianceDisplaySelectionHandlerPtr; /** @brief Displays the pose from a geometry_msgs::PoseWithCovarianceStamped message. */ -class PoseWithCovarianceDisplay: public MessageFilterDisplay +class PoseWithCovarianceDisplay: public rviz::MessageFilterDisplay { Q_OBJECT public: @@ -86,24 +87,24 @@ private Q_SLOTS: virtual void processMessage( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& message ); - Arrow* arrow_; - Axes* axes_; + rviz::Arrow* arrow_; + rviz::Axes* axes_; boost::shared_ptr covariance_; bool pose_valid_; PoseWithCovarianceDisplaySelectionHandlerPtr coll_handler_; - EnumProperty* shape_property_; + rviz::EnumProperty* shape_property_; - ColorProperty* color_property_; - FloatProperty* alpha_property_; + rviz::ColorProperty* color_property_; + rviz::FloatProperty* alpha_property_; - FloatProperty* head_radius_property_; - FloatProperty* head_length_property_; - FloatProperty* shaft_radius_property_; - FloatProperty* shaft_length_property_; + rviz::FloatProperty* head_radius_property_; + rviz::FloatProperty* head_length_property_; + rviz::FloatProperty* shaft_radius_property_; + rviz::FloatProperty* shaft_length_property_; - FloatProperty* axes_length_property_; - FloatProperty* axes_radius_property_; + rviz::FloatProperty* axes_length_property_; + rviz::FloatProperty* axes_radius_property_; CovarianceProperty* covariance_property_; From bcdd11e3f1c5a39c89175a65809593007e55a3f4 Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Wed, 3 May 2017 14:56:21 +0200 Subject: [PATCH 3/6] Add offset to covariance property While here, fix end of namespace comment --- .../default_plugin/covariance_property.cpp | 45 ++++++++++--------- src/rviz/default_plugin/covariance_property.h | 5 ++- 2 files changed, 28 insertions(+), 22 deletions(-) diff --git a/src/rviz/default_plugin/covariance_property.cpp b/src/rviz/default_plugin/covariance_property.cpp index 5586822f17..611c3418bc 100644 --- a/src/rviz/default_plugin/covariance_property.cpp +++ b/src/rviz/default_plugin/covariance_property.cpp @@ -59,17 +59,17 @@ CovarianceProperty::CovarianceProperty( const QString& name, position_color_property_ = new ColorProperty( "Color", QColor( 204, 51, 204 ), "Color to draw the position covariance ellipse.", - position_property_, SLOT( updateColorAndAlphaAndScale() ), this ); - + position_property_, SLOT( updateColorAndAlphaAndScaleAndOffset() ), this ); + position_alpha_property_ = new FloatProperty( "Alpha", 0.3f, "0 is fully transparent, 1.0 is fully opaque.", - position_property_, SLOT( updateColorAndAlphaAndScale() ), this ); + position_property_, SLOT( updateColorAndAlphaAndScaleAndOffset() ), this ); position_alpha_property_->setMin( 0 ); position_alpha_property_->setMax( 1 ); - + position_scale_property_ = new FloatProperty( "Scale", 1.0f, - "Scale factor to be applied to covariance ellipse", - position_property_, SLOT( updateColorAndAlphaAndScale() ), this ); + "Scale factor to be applied to covariance ellipse. Corresponds to the number of standard deviations to display", + position_property_, SLOT( updateColorAndAlphaAndScaleAndOffset() ), this ); position_scale_property_->setMin( 0 ); orientation_property_ = new BoolProperty( "Orientation", true, @@ -89,17 +89,22 @@ CovarianceProperty::CovarianceProperty( const QString& name, orientation_color_property_ = new ColorProperty( "Color", QColor( 255, 255, 127 ), "Color to draw the covariance ellipse.", - orientation_property_, SLOT( updateColorAndAlphaAndScale() ), this ); - + orientation_property_, SLOT( updateColorAndAlphaAndScaleAndOffset() ), this ); + orientation_alpha_property_ = new FloatProperty( "Alpha", 0.5f, "0 is fully transparent, 1.0 is fully opaque.", - orientation_property_, SLOT( updateColorAndAlphaAndScale() ), this ); + orientation_property_, SLOT( updateColorAndAlphaAndScaleAndOffset() ), this ); orientation_alpha_property_->setMin( 0 ); orientation_alpha_property_->setMax( 1 ); - + + orientation_offset_property_ = new FloatProperty( "Offset", 1.0f, + "For 3D poses is the distance where to position the ellipses representing orientation covariance. For 2D poses is the height of the triangle representing the variance on yaw", + orientation_property_, SLOT( updateColorAndAlphaAndScaleAndOffset() ), this ); + orientation_offset_property_->setMin( 0 ); + orientation_scale_property_ = new FloatProperty( "Scale", 1.0f, - "For 3D poses is the distance where to position orientation covariance. For 2D poses is the size of the triangle representing covariance on yaw", - orientation_property_, SLOT( updateColorAndAlphaAndScale() ), this ); + "Scale factor to be applied to orientation covariance shapes. Corresponds to the number of standard deviations to display", + orientation_property_, SLOT( updateColorAndAlphaAndScaleAndOffset() ), this ); orientation_scale_property_->setMin( 0 ); connect(this, SIGNAL( changed() ), this, SLOT( updateVisibility() )); @@ -125,18 +130,18 @@ void CovarianceProperty::updateColorStyleChoice() { bool use_unique_color = ( orientation_colorstyle_property_->getOptionInt() == Unique ); orientation_color_property_->setHidden( !use_unique_color ); - updateColorAndAlphaAndScale(); + updateColorAndAlphaAndScaleAndOffset(); } -void CovarianceProperty::updateColorAndAlphaAndScale() +void CovarianceProperty::updateColorAndAlphaAndScaleAndOffset() { D_Covariance::iterator it_cov = covariances_.begin(); D_Covariance::iterator end_cov = covariances_.end(); for ( ; it_cov != end_cov; ++it_cov ) - updateColorAndAlphaAndScale(*it_cov); + updateColorAndAlphaAndScaleAndOffset(*it_cov); } -void CovarianceProperty::updateColorAndAlphaAndScale(const CovarianceVisualPtr& visual) +void CovarianceProperty::updateColorAndAlphaAndScaleAndOffset(const CovarianceVisualPtr& visual) { float pos_alpha = position_alpha_property_->getFloat(); float pos_scale = position_scale_property_->getFloat(); @@ -145,6 +150,7 @@ void CovarianceProperty::updateColorAndAlphaAndScale(const CovarianceVisualPtr& visual->setPositionScale( pos_scale ); float ori_alpha = orientation_alpha_property_->getFloat(); + float ori_offset = orientation_offset_property_->getFloat(); float ori_scale = orientation_scale_property_->getFloat(); if(orientation_colorstyle_property_->getOptionInt() == Unique) { @@ -155,6 +161,7 @@ void CovarianceProperty::updateColorAndAlphaAndScale(const CovarianceVisualPtr& { visual->setOrientationColorToRGB( ori_alpha ); } + visual->setOrientationOffset( ori_offset ); visual->setOrientationScale( ori_scale ); } @@ -217,7 +224,7 @@ CovarianceProperty::CovarianceVisualPtr CovarianceProperty::createAndPushBackVis CovarianceVisualPtr visual(new CovarianceVisual(scene_manager, parent_node, use_rotating_frame) ); updateVisibility(visual); updateOrientationFrame(visual); - updateColorAndAlphaAndScale(visual); + updateColorAndAlphaAndScaleAndOffset(visual); covariances_.push_back(visual); return visual; } @@ -232,6 +239,4 @@ bool CovarianceProperty::getOrientationBool() return orientation_property_->getBool(); } - - -} // end namespace rviz_plugin_covariance +} // end namespace rviz diff --git a/src/rviz/default_plugin/covariance_property.h b/src/rviz/default_plugin/covariance_property.h index 0461ead95c..8436697843 100644 --- a/src/rviz/default_plugin/covariance_property.h +++ b/src/rviz/default_plugin/covariance_property.h @@ -92,12 +92,12 @@ public Q_SLOTS: void updateVisibility(); private Q_SLOTS: - void updateColorAndAlphaAndScale(); + void updateColorAndAlphaAndScaleAndOffset(); void updateOrientationFrame(); void updateColorStyleChoice(); private: - void updateColorAndAlphaAndScale( const CovarianceVisualPtr& visual ); + void updateColorAndAlphaAndScaleAndOffset( const CovarianceVisualPtr& visual ); void updateOrientationFrame( const CovarianceVisualPtr& visual ); void updateVisibility( const CovarianceVisualPtr& visual ); @@ -113,6 +113,7 @@ private Q_SLOTS: rviz::EnumProperty* orientation_colorstyle_property_; rviz::ColorProperty* orientation_color_property_; rviz::FloatProperty* orientation_alpha_property_; + rviz::FloatProperty* orientation_offset_property_; rviz::FloatProperty* orientation_scale_property_; }; From de0872f9552f447bb4f5f15fa6883f80fde3c645 Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Wed, 3 May 2017 15:10:27 +0200 Subject: [PATCH 4/6] Refactor CovarianceVisual - Declare rviz objects explicitly in the header - Add offset for orientation (used to position orientation shapes) - Rename orientation_scale_node_ to orientation_root_node_ - Limit the angle to be displayed to 89 deg - Add helper functions in anonymoous namespace --- src/rviz/default_plugin/covariance_visual.cpp | 275 +++++++++++------- src/rviz/default_plugin/covariance_visual.h | 25 +- 2 files changed, 181 insertions(+), 119 deletions(-) diff --git a/src/rviz/default_plugin/covariance_visual.cpp b/src/rviz/default_plugin/covariance_visual.cpp index fb99c4d466..80746d53a8 100644 --- a/src/rviz/default_plugin/covariance_visual.cpp +++ b/src/rviz/default_plugin/covariance_visual.cpp @@ -29,11 +29,10 @@ #include "covariance_visual.h" -#include +#include "rviz/ogre_helpers/shape.h" #include #include -#include #include #include @@ -43,89 +42,12 @@ namespace rviz { -double deg2rad (double degrees) { - return degrees * 4.0 * atan (1.0) / 180.0; -} - -CovarianceVisual::CovarianceVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation, bool is_visible, float pos_scale, float ori_scale) -: Object( scene_manager ), local_rotation_(is_local_rotation), pose_2d_(false), orientation_visible_(is_visible) +namespace { - // Main node of the visual - root_node_ = parent_node->createChildSceneNode(); - // Node that will have the same orientation as the fixed frame. Updated from the message on setCovariance() - fixed_orientation_node_ = root_node_->createChildSceneNode(); - // Node to scale the position part of the covariance from the property value - position_scale_node_ = fixed_orientation_node_->createChildSceneNode(); - // Node to be oriented and scaled from the message's covariance - position_node_ = position_scale_node_->createChildSceneNode(); - position_shape_ = new Shape(Shape::Sphere, scene_manager_, position_node_); - - // Node to scale the orientation part of the covariance. May be attached to both the local (root) node or the fixed frame node. - // May be re-attached later by setRotatingFrame() - if(local_rotation_) - orientation_scale_node_ = root_node_->createChildSceneNode(); - else - orientation_scale_node_ = fixed_orientation_node_->createChildSceneNode(); - - for(int i = 0; i < kNumOriShapes; i++) - { - // Node to position and orient the shape along the axis. One for each axis. - orientation_offset_node_[i] = orientation_scale_node_->createChildSceneNode(); - // Does not inherit scale from the parent. This is needed to keep the cylinders with the same height. The scale is set by setOrientationScale() - orientation_offset_node_[i]->setInheritScale( false ); - // Node to be oriented and scaled by the message's covariance. One for each axis. - orientation_node_[i] = orientation_offset_node_[i]->createChildSceneNode(); - if(i != kYaw2D) - orientation_shape_[i] = new Shape(Shape::Cylinder, scene_manager_, orientation_node_[i]); - else - orientation_shape_[i] = new Shape(Shape::Cone, scene_manager_, orientation_node_[i]); - } - // Position the cylindes at position 1.0 in the respective axis, and perpendicular to the axis. - // x-axis (roll) - orientation_offset_node_[kRoll]->setPosition( Ogre::Vector3::UNIT_X ); - orientation_offset_node_[kRoll]->setOrientation( Ogre::Quaternion(Ogre::Degree( 90 ), Ogre::Vector3::UNIT_X ) * Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Z ) ); - // y-axis (pitch) - orientation_offset_node_[kPitch]->setPosition( Ogre::Vector3( Ogre::Vector3::UNIT_Y ) ); - orientation_offset_node_[kPitch]->setOrientation( Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Y ) ); - // z-axis (yaw) - orientation_offset_node_[kYaw]->setPosition( Ogre::Vector3( Ogre::Vector3::UNIT_Z ) ); - orientation_offset_node_[kYaw]->setOrientation( Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_X ) ); - // z-axis (yaw 2D) - // NOTE: rviz use a cone defined by the file rviz/ogre_media/models/rviz_cone.mesh, and it's - // origin is not at the top of the cone. Since we want the top to be at the origin of - // the pose we need to use an offset here. - // WARNING: This number was found by trial-and-error on rviz and it's not the correct - // one, so changes on scale are expected to cause the top of the cone to move - // from the pose origin, although it's only noticeable with big scales. - // FIXME: Find the right value from the cone.mesh file, or implement a class that draws - // something like a 2D "pie slice" and use it instead of the cone. - static const double cone_origin_to_top = 0.49115; - orientation_offset_node_[kYaw2D]->setPosition(cone_origin_to_top * Ogre::Vector3::UNIT_X); - orientation_offset_node_[kYaw2D]->setOrientation(Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Z )); - - // set initial visibility and scale - // root node is always visible. The visibility will be updated on its childs. - root_node_->setVisible( true ); - setVisible( is_visible ); - setScales( pos_scale, ori_scale ); -} - -CovarianceVisual::~CovarianceVisual() +double deg2rad (double degrees) { - delete position_shape_; - scene_manager_->destroySceneNode( position_node_->getName() ); - - for(int i = 0; i < kNumOriShapes; i++) - { - delete orientation_shape_[i]; - scene_manager_->destroySceneNode( orientation_node_[i]->getName() ); - scene_manager_->destroySceneNode( orientation_offset_node_[i]->getName() ); - } - - scene_manager_->destroySceneNode( position_scale_node_->getName() ); - scene_manager_->destroySceneNode( fixed_orientation_node_->getName() ); - scene_manager_->destroySceneNode( root_node_->getName() ); + return degrees * 4.0 * atan (1.0) / 180.0; } // Local function to force the axis to be right handed for 3D. Taken from ecl_statistics @@ -262,13 +184,101 @@ void computeShapeScaleAndOrientation2D(const Eigen::Matrix2d& covariance, Ogre:: } } -void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale) +void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale, float max_degrees) { radian_scale /= 2.0; - if(radian_scale > deg2rad(85.0)) radian_scale = deg2rad(85.0); + if(radian_scale > deg2rad(max_degrees)) radian_scale = deg2rad(max_degrees); radian_scale = 2.0 * tan(radian_scale); } + +} + +const float CovarianceVisual::max_degrees = 89.0; + +CovarianceVisual::CovarianceVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation, bool is_visible, float pos_scale, float ori_scale, float ori_offset) +: Object( scene_manager ), local_rotation_(is_local_rotation), pose_2d_(false), orientation_visible_(is_visible) +{ + // Main node of the visual + root_node_ = parent_node->createChildSceneNode(); + // Node that will have the same orientation as the fixed frame. Updated from the message on setCovariance() + fixed_orientation_node_ = root_node_->createChildSceneNode(); + // Node to scale the position part of the covariance from the property value + position_scale_node_ = fixed_orientation_node_->createChildSceneNode(); + // Node to be oriented and scaled from the message's covariance + position_node_ = position_scale_node_->createChildSceneNode(); + position_shape_ = new rviz::Shape(rviz::Shape::Sphere, scene_manager_, position_node_); + + // Node to scale the orientation part of the covariance. May be attached to both the local (root) node or the fixed frame node. + // May be re-attached later by setRotatingFrame() + if(local_rotation_) + orientation_root_node_ = root_node_->createChildSceneNode(); + else + orientation_root_node_ = fixed_orientation_node_->createChildSceneNode(); + + for(int i = 0; i < kNumOriShapes; i++) + { + // Node to position and orient the shape along the axis. One for each axis. + orientation_offset_node_[i] = orientation_root_node_->createChildSceneNode(); + // Does not inherit scale from the parent. This is needed to keep the cylinders with the same height. The scale is set by setOrientationScale() + orientation_offset_node_[i]->setInheritScale( false ); + + if(i != kYaw2D) + orientation_shape_[i] = new rviz::Shape(rviz::Shape::Cylinder, scene_manager_, orientation_offset_node_[i]); + else + orientation_shape_[i] = new rviz::Shape(rviz::Shape::Cone, scene_manager_, orientation_offset_node_[i]); + + // Initialize all current scales to 0 + current_ori_scale_[i] = Ogre::Vector3(0,0,0); + } + + // Position the cylindes at position 1.0 in the respective axis, and perpendicular to the axis. + // x-axis (roll) + orientation_offset_node_[kRoll]->setPosition( Ogre::Vector3::UNIT_X ); + orientation_offset_node_[kRoll]->setOrientation( Ogre::Quaternion(Ogre::Degree( 90 ), Ogre::Vector3::UNIT_X ) * Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Z ) ); + // y-axis (pitch) + orientation_offset_node_[kPitch]->setPosition( Ogre::Vector3( Ogre::Vector3::UNIT_Y ) ); + orientation_offset_node_[kPitch]->setOrientation( Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Y ) ); + // z-axis (yaw) + orientation_offset_node_[kYaw]->setPosition( Ogre::Vector3( Ogre::Vector3::UNIT_Z ) ); + orientation_offset_node_[kYaw]->setOrientation( Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_X ) ); + // z-axis (yaw 2D) + // NOTE: rviz use a cone defined by the file rviz/ogre_media/models/rviz_cone.mesh, and it's + // origin is not at the top of the cone. Since we want the top to be at the origin of + // the pose we need to use an offset here. + // WARNING: This number was found by trial-and-error on rviz and it's not the correct + // one, so changes on scale are expected to cause the top of the cone to move + // from the pose origin, although it's only noticeable with big scales. + // FIXME: Find the right value from the cone.mesh file, or implement a class that draws + // something like a 2D "pie slice" and use it instead of the cone. + static const double cone_origin_to_top = 0.49115; + orientation_offset_node_[kYaw2D]->setPosition(cone_origin_to_top * Ogre::Vector3::UNIT_X); + orientation_offset_node_[kYaw2D]->setOrientation(Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Z )); + + // set initial visibility and scale + // root node is always visible. The visibility will be updated on its childs. + root_node_->setVisible( true ); + setVisible( is_visible ); + setScales( pos_scale, ori_scale ); + setOrientationOffset( ori_offset ); +} + +CovarianceVisual::~CovarianceVisual() +{ + delete position_shape_; + scene_manager_->destroySceneNode( position_node_->getName() ); + + for(int i = 0; i < kNumOriShapes; i++) + { + delete orientation_shape_[i]; + scene_manager_->destroySceneNode( orientation_offset_node_[i]->getName() ); + } + + scene_manager_->destroySceneNode( position_scale_node_->getName() ); + scene_manager_->destroySceneNode( fixed_orientation_node_->getName() ); + scene_manager_->destroySceneNode( root_node_->getName() ); +} + void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& pose ) { // check for NaN in covariance @@ -290,10 +300,10 @@ void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& p // store orientation in Ogre structure Ogre::Quaternion ori(pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z); - // Set the orientation of the fixed node. Since this node is attached to the root node, it's orientation will be the + // Set the orientation of the fixed node. Since this node is attached to the root node, it's orientation will be the // inverse of pose's orientation. fixed_orientation_node_->setOrientation(ori.Inverse()); - // Map covariance to a Eigen::Matrix + // Map covariance to a Eigen::Matrix Eigen::Map > covariance(pose.covariance.data()); updatePosition(covariance); @@ -319,7 +329,7 @@ void CovarianceVisual::updatePosition( const Eigen::Matrix6d& covariance ) { computeShapeScaleAndOrientation2D(covariance.topLeftCorner<2,2>(), shape_scale, shape_orientation, XY_PLANE); // Make the scale in z minimal for better visualization - shape_scale.z = 0.001; + shape_scale.z = 0.001; } else { @@ -344,14 +354,18 @@ void CovarianceVisual::updateOrientation( const Eigen::Matrix6d& covariance, Sha assert(index == kYaw2D); // 2D poses only depend on yaw. shape_scale.x = 2.0*sqrt(covariance(5,5)); - // The the covariance is equivalent to twice the standard deviation _in radians_. - // So we need to convert it to the linear scale of the shape using tan(). - // Also, we bound the maximum std to 85 deg. - radianScaleToMetricScaleBounded(shape_scale.x); // To display the cone shape properly the scale along y-axis has to be one. shape_scale.y = 1.0; // Give a minimal height for the cone for better visualization shape_scale.z = 0.001; + // Store the computed scale to be used if the user change the scale + current_ori_scale_[index] = shape_scale; + // Apply the current scale factor + shape_scale.x *= current_ori_scale_factor_; + // The scale on x means twice the standard deviation, but _in radians_. + // So we need to convert it to the linear scale of the shape using tan(). + // Also, we bound the maximum std + radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); } else { @@ -374,19 +388,24 @@ void CovarianceVisual::updateOrientation( const Eigen::Matrix6d& covariance, Sha // NOTE: The cylinder mesh is oriented along its y axis, thus we want to flat it out into the XZ plane computeShapeScaleAndOrientation2D(covarianceAxis, shape_scale, shape_orientation, XZ_PLANE); + // Give a minimal height for the cylinder for better visualization + shape_scale.y = 0.001; + // Store the computed scale to be used if the user change the scale + current_ori_scale_[index] = shape_scale; + // Apply the current scale factor + shape_scale.x *= current_ori_scale_factor_; + shape_scale.z *= current_ori_scale_factor_; // The computed scale is equivalent to twice the standard deviation _in radians_. // So we need to convert it to the linear scale of the shape using tan(). - // Also, we bound the maximum std to 85 deg. - radianScaleToMetricScaleBounded(shape_scale.x); - radianScaleToMetricScaleBounded(shape_scale.z); - // Give a minimal height for the cylinder for better visualization - shape_scale.y = 0.001; + // Also, we bound the maximum std. + radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); + radianScaleToMetricScaleBounded(shape_scale.z, max_degrees); } // Rotate and scale the scene node of the orientation part - orientation_node_[index]->setOrientation(shape_orientation); + orientation_shape_[index]->setOrientation(shape_orientation); if(!shape_scale.isNaN()) - orientation_node_[index]->setScale(shape_scale); + orientation_shape_[index]->setScale(shape_scale); else ROS_WARN_STREAM("orientation shape_scale contains NaN: " << shape_scale); } @@ -397,7 +416,7 @@ void CovarianceVisual::setScales( float pos_scale, float ori_scale) setOrientationScale(ori_scale); } -void CovarianceVisual::setPositionScale( float pos_scale ) +void CovarianceVisual::setPositionScale( float pos_scale ) { if(pose_2d_) position_scale_node_->setScale( pos_scale, pos_scale, 1.0 ); @@ -405,23 +424,59 @@ void CovarianceVisual::setPositionScale( float pos_scale ) position_scale_node_->setScale( pos_scale, pos_scale, pos_scale ); } +void CovarianceVisual::setOrientationOffset( float ori_offset ) +{ + // Scale the orientation root node to position the shapes along the axis + orientation_root_node_->setScale( ori_offset, ori_offset, ori_offset ); + // The scale the offset_nodes as well so the displayed shape represents a 1-sigma + // standard deviation when displayed with an scale of 1.0 + // NOTE: We only want to change the scales of the dimentions that represent the + // orientation covariance. The other dimensions are set to 1.0. + for(int i = 0; i < kNumOriShapes; i++) + { + if(i == kYaw2D) + { + // For 2D, the angle is only encoded on x, but we also scale on y to put the top of the cone at the pose origin + orientation_offset_node_[i]->setScale( ori_offset, ori_offset, 1.0 ); + } + else + { + // For 3D, the angle covariance is encoded on x and z dimensions + orientation_offset_node_[i]->setScale( ori_offset, 1.0, ori_offset ); + } + } +} + void CovarianceVisual::setOrientationScale( float ori_scale ) { - // Scale the orientation scale node to position the shapes along the axis - orientation_scale_node_->setScale( ori_scale, ori_scale, ori_scale ); - // The scale along the flatten out dimension is always 1.0 + // Here we update the current scale factor, apply it to the current scale _in radians_, + // convert it to meters and apply to the shape scale. Note we have different invariant + // scales in the 3D and in 2D. + current_ori_scale_factor_ = ori_scale; for(int i = 0; i < kNumOriShapes; i++) { + // Recover the last computed scale + Ogre::Vector3 shape_scale = current_ori_scale_[i]; if(i == kYaw2D) { - // To proper flat the cone in the 2D case we flat out along z - orientation_offset_node_[i]->setScale( ori_scale, ori_scale, 1.0 ); + // Changes in scale in 2D only affects the x dimension + // Apply the current scale factor + shape_scale.x *= current_ori_scale_factor_; + // Convert from radians to meters + radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); } else { - // For the cylinder in 3D case is the y-axis - orientation_offset_node_[i]->setScale( ori_scale, 1.0, ori_scale ); + // Changes in scale in 3D only affects the x and z dimensions + // Apply the current scale factor + shape_scale.x *= current_ori_scale_factor_; + shape_scale.z *= current_ori_scale_factor_; + // Convert from radians to meters + radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); + radianScaleToMetricScaleBounded(shape_scale.z, max_degrees); } + // Apply the new scale + orientation_shape_[i]->setScale(shape_scale); } } @@ -501,7 +556,7 @@ void CovarianceVisual::updateOrientationVisibility() } -const Ogre::Vector3& CovarianceVisual::getPosition() +const Ogre::Vector3& CovarianceVisual::getPosition() { return position_node_->getPosition(); } @@ -529,13 +584,13 @@ void CovarianceVisual::setRotatingFrame( bool is_local_rotation ) local_rotation_ = is_local_rotation; if(local_rotation_) - root_node_->addChild( fixed_orientation_node_->removeChild( orientation_scale_node_->getName() ) ); + root_node_->addChild( fixed_orientation_node_->removeChild( orientation_root_node_->getName() ) ); else - fixed_orientation_node_->addChild( root_node_->removeChild( orientation_scale_node_->getName() ) ); + fixed_orientation_node_->addChild( root_node_->removeChild( orientation_root_node_->getName() ) ); } -Shape* CovarianceVisual::getOrientationShape(ShapeIndex index) +rviz::Shape* CovarianceVisual::getOrientationShape(ShapeIndex index) { return orientation_shape_[index]; } diff --git a/src/rviz/default_plugin/covariance_visual.h b/src/rviz/default_plugin/covariance_visual.h index da56fbc23b..0b89038a8b 100644 --- a/src/rviz/default_plugin/covariance_visual.h +++ b/src/rviz/default_plugin/covariance_visual.h @@ -40,6 +40,7 @@ #include +#include #include using std::isnan; @@ -58,6 +59,7 @@ namespace Eigen namespace rviz { + class Shape; class CovarianceProperty; @@ -65,7 +67,7 @@ class CovarianceProperty; * \class CovarianceVisual * \brief CovarianceVisual consisting in a ellipse for position and 2D ellipses along the axis for orientation. */ -class CovarianceVisual : public Object +class CovarianceVisual : public rviz::Object { public: enum ShapeIndex @@ -90,7 +92,7 @@ class CovarianceVisual : public Object * @param pos_scale Scale of the position covariance * @param ori_scale Scale of the orientation covariance */ - 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); + 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); public: virtual ~CovarianceVisual(); @@ -102,6 +104,7 @@ class CovarianceVisual : public Object */ void setScales( float pos_scale, float ori_scale); void setPositionScale( float pos_scale ); + void setOrientationOffset( float ori_offset ); void setOrientationScale( float ori_scale ); /** @@ -145,19 +148,19 @@ class CovarianceVisual : public Object * \brief Get the root scene node of the orientation part of this covariance * @return the root scene node of the orientation part of this covariance */ - Ogre::SceneNode* getOrientationSceneNode() { return orientation_scale_node_; } + Ogre::SceneNode* getOrientationSceneNode() { return orientation_root_node_; } /** * \brief Get the shape used to display position covariance * @return the shape used to display position covariance */ - Shape* getPositionShape() { return position_shape_; } + rviz::Shape* getPositionShape() { return position_shape_; } /** * \brief Get the shape used to display orientation covariance in an especific axis * @return the shape used to display orientation covariance in an especific axis */ - Shape* getOrientationShape(ShapeIndex index); + rviz::Shape* getOrientationShape(ShapeIndex index); /** * \brief Sets user data on all ogre objects we own @@ -206,12 +209,11 @@ class CovarianceVisual : public Object Ogre::SceneNode* position_scale_node_; Ogre::SceneNode* position_node_; - Ogre::SceneNode* orientation_scale_node_; + Ogre::SceneNode* orientation_root_node_; Ogre::SceneNode* orientation_offset_node_[kNumOriShapes]; - Ogre::SceneNode* orientation_node_[kNumOriShapes]; - Shape* position_shape_; ///< Ellipse used for the position covariance - Shape* orientation_shape_[kNumOriShapes]; ///< Cylinders used for the orientation covariance + rviz::Shape* position_shape_; ///< Ellipse used for the position covariance + rviz::Shape* orientation_shape_[kNumOriShapes]; ///< Cylinders used for the orientation covariance bool local_rotation_; @@ -219,6 +221,11 @@ class CovarianceVisual : public Object bool orientation_visible_; ///< If the orientation component is visible. + Ogre::Vector3 current_ori_scale_[kNumOriShapes]; + float current_ori_scale_factor_; + + const static float max_degrees; + private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... From ea5d9a27d8d81d2592c6336ad582ebe4611c4ea7 Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Wed, 3 May 2017 15:23:13 +0200 Subject: [PATCH 5/6] Fix tolerance test at angular discontinuity --- src/rviz/default_plugin/odometry_display.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index 6518e7f240..3c86b85f99 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/src/rviz/default_plugin/odometry_display.cpp @@ -267,12 +267,11 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag { 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); Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z); - Ogre::Quaternion 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); - Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z); + 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); + Eigen::Quaternionf current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z); - // FIXME: the angle tolerance test does not work at the angular discontinuity if( (last_position - current_position).length() < position_tolerance_property_->getFloat() && - (last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() ) + last_orientation.angularDistance(current_orientation) < angle_tolerance_property_->getFloat() ) { return; } From 73883c7ec8791176601a949c2fa150060ca84254 Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Wed, 3 May 2017 15:30:05 +0200 Subject: [PATCH 6/6] Rename PoseWithCovarianceDisplay::Shape enums --- .../default_plugin/pose_with_covariance_display.cpp | 10 +++++----- src/rviz/default_plugin/pose_with_covariance_display.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/rviz/default_plugin/pose_with_covariance_display.cpp b/src/rviz/default_plugin/pose_with_covariance_display.cpp index 361aa20f15..c8ab2ff615 100644 --- a/src/rviz/default_plugin/pose_with_covariance_display.cpp +++ b/src/rviz/default_plugin/pose_with_covariance_display.cpp @@ -87,7 +87,7 @@ class PoseWithCovarianceDisplaySelectionHandler: public SelectionHandler { if( display_->pose_valid_ ) { - if( display_->shape_property_->getOptionInt() == PoseWithCovarianceDisplay::ArrowShape ) + if( display_->shape_property_->getOptionInt() == PoseWithCovarianceDisplay::Arrow ) { aabbs.push_back( display_->arrow_->getHead()->getEntity()->getWorldBoundingBox() ); aabbs.push_back( display_->arrow_->getShaft()->getEntity()->getWorldBoundingBox() ); @@ -156,8 +156,8 @@ PoseWithCovarianceDisplay::PoseWithCovarianceDisplay() { shape_property_ = new EnumProperty( "Shape", "Arrow", "Shape to display the pose as.", this, SLOT( updateShapeChoice() )); - shape_property_->addOption( "Arrow", ArrowShape ); - shape_property_->addOption( "Axes", AxesShape ); + shape_property_->addOption( "Arrow", Arrow ); + shape_property_->addOption( "Axes", Axes ); color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrow.", this, SLOT( updateColorAndAlpha() )); @@ -263,7 +263,7 @@ void PoseWithCovarianceDisplay::updateAxisGeometry() void PoseWithCovarianceDisplay::updateShapeChoice() { - bool use_arrow = ( shape_property_->getOptionInt() == ArrowShape ); + bool use_arrow = ( shape_property_->getOptionInt() == Arrow ); color_property_->setHidden( !use_arrow ); alpha_property_->setHidden( !use_arrow ); @@ -290,7 +290,7 @@ void PoseWithCovarianceDisplay::updateShapeVisibility() } else { - bool use_arrow = (shape_property_->getOptionInt() == ArrowShape); + bool use_arrow = (shape_property_->getOptionInt() == Arrow); arrow_->getSceneNode()->setVisible( use_arrow ); axes_->getSceneNode()->setVisible( !use_arrow ); covariance_property_->updateVisibility(); diff --git a/src/rviz/default_plugin/pose_with_covariance_display.h b/src/rviz/default_plugin/pose_with_covariance_display.h index e1e7c105dc..47103f2ae9 100644 --- a/src/rviz/default_plugin/pose_with_covariance_display.h +++ b/src/rviz/default_plugin/pose_with_covariance_display.h @@ -61,8 +61,8 @@ Q_OBJECT public: enum Shape { - ArrowShape, - AxesShape, + Arrow, + Axes, }; PoseWithCovarianceDisplay();