diff --git a/plugin_description.xml b/plugin_description.xml index 64152baa76..fb0152d531 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -127,6 +127,12 @@ geometry_msgs/PoseArray + + + Displays a geometry_msgs::PoseWithCovarianceStamped message. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/PoseWithCovariance">More Information</a>. + + geometry_msgs/PoseWithCovarianceStamped + Displays the data from sensor_msgs::Range messages as cones. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/Range">More Information</a> diff --git a/src/rviz/default_plugin/CMakeLists.txt b/src/rviz/default_plugin/CMakeLists.txt index fe096270f5..989ebc156a 100644 --- a/src/rviz/default_plugin/CMakeLists.txt +++ b/src/rviz/default_plugin/CMakeLists.txt @@ -3,6 +3,8 @@ include_directories(.) set(SOURCE_FILES axes_display.cpp camera_display.cpp + covariance_visual.cpp + covariance_property.cpp depth_cloud_display.cpp depth_cloud_mld.cpp effort_display.cpp @@ -42,6 +44,7 @@ set(SOURCE_FILES polygon_display.cpp pose_array_display.cpp pose_display.cpp + pose_with_covariance_display.cpp range_display.cpp relative_humidity_display.cpp robot_model_display.cpp diff --git a/src/rviz/default_plugin/covariance_property.cpp b/src/rviz/default_plugin/covariance_property.cpp new file mode 100644 index 0000000000..ee9b77135c --- /dev/null +++ b/src/rviz/default_plugin/covariance_property.cpp @@ -0,0 +1,237 @@ +/* + * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "covariance_property.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" + +#include + +#include +#include + +namespace rviz +{ + +CovarianceProperty::CovarianceProperty( const QString& name, + bool default_value, + const QString& description, + Property* parent, + const char *changed_slot, + QObject* receiver ) + // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of this constructor + : BoolProperty( name, default_value, description, parent ) +{ + + position_property_ = new BoolProperty( "Position", true, + "Whether or not to show the position part of covariances", + this, SLOT( updateVisibility() )); + position_property_->setDisableChildrenIfFalse( true ); + + position_color_property_ = new ColorProperty( "Color", QColor( 204, 51, 204 ), + "Color to draw the position covariance ellipse.", + position_property_, SLOT( updateColorAndAlphaAndScale() ), this ); + + position_alpha_property_ = new FloatProperty( "Alpha", 0.3f, + "0 is fully transparent, 1.0 is fully opaque.", + position_property_, SLOT( updateColorAndAlphaAndScale() ), 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 ); + position_scale_property_->setMin( 0 ); + + orientation_property_ = new BoolProperty( "Orientation", true, + "Whether or not to show the orientation part of covariances", + this, SLOT( updateVisibility() )); + orientation_property_->setDisableChildrenIfFalse( true ); + + orientation_frame_property_ = new EnumProperty( "Frame", "Local", "The frame used to display the orientation covariance.", + orientation_property_, SLOT( updateOrientationFrame() ), this ); + orientation_frame_property_->addOption( "Local", Local ); + orientation_frame_property_->addOption( "Fixed", Fixed ); + + orientation_colorstyle_property_ = new EnumProperty( "Color Style", "Unique", "Style to color the orientation covariance: XYZ with same unique color or following RGB order", + orientation_property_, SLOT( updateColorStyleChoice() ), this ); + orientation_colorstyle_property_->addOption( "Unique", Unique ); + orientation_colorstyle_property_->addOption( "RGB", RGB ); + + orientation_color_property_ = new ColorProperty( "Color", QColor( 255, 255, 127 ), + "Color to draw the covariance ellipse.", + orientation_property_, SLOT( updateColorAndAlphaAndScale() ), this ); + + orientation_alpha_property_ = new FloatProperty( "Alpha", 0.5f, + "0 is fully transparent, 1.0 is fully opaque.", + orientation_property_, SLOT( updateColorAndAlphaAndScale() ), this ); + orientation_alpha_property_->setMin( 0 ); + orientation_alpha_property_->setMax( 1 ); + + 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 ); + orientation_scale_property_->setMin( 0 ); + + connect(this, SIGNAL( changed() ), this, SLOT( updateVisibility() )); + + // Connect changed() signal here instead of doing it through the initialization of BoolProperty(). + // We do this here to make changed_slot be called _after_ updateVisibility() + if(changed_slot && (parent || receiver)) + { + if(receiver) + connect(this, SIGNAL( changed() ), receiver, changed_slot); + else + connect(this, SIGNAL( changed() ), parent, changed_slot); + } + + setDisableChildrenIfFalse( true ); +} + +CovarianceProperty::~CovarianceProperty() +{ +} + +void CovarianceProperty::updateColorStyleChoice() +{ + bool use_unique_color = ( orientation_colorstyle_property_->getOptionInt() == Unique ); + orientation_color_property_->setHidden( !use_unique_color ); + updateColorAndAlphaAndScale(); +} + +void CovarianceProperty::updateColorAndAlphaAndScale() +{ + D_Covariance::iterator it_cov = covariances_.begin(); + D_Covariance::iterator end_cov = covariances_.end(); + for ( ; it_cov != end_cov; ++it_cov ) + updateColorAndAlphaAndScale(*it_cov); +} + +void CovarianceProperty::updateColorAndAlphaAndScale(const CovarianceVisualPtr& visual) +{ + float pos_alpha = position_alpha_property_->getFloat(); + float pos_scale = position_scale_property_->getFloat(); + QColor pos_color = position_color_property_->getColor(); + visual->setPositionColor( pos_color.redF(), pos_color.greenF(), pos_color.blueF(), pos_alpha ); + visual->setPositionScale( pos_scale ); + + float ori_alpha = orientation_alpha_property_->getFloat(); + float ori_scale = orientation_scale_property_->getFloat(); + if(orientation_colorstyle_property_->getOptionInt() == Unique) + { + QColor ori_color = orientation_color_property_->getColor(); + visual->setOrientationColor( ori_color.redF(), ori_color.greenF(), ori_color.blueF(), ori_alpha ); + } + else + { + visual->setOrientationColorToRGB( ori_alpha ); + } + visual->setOrientationScale( ori_scale ); +} + +void CovarianceProperty::updateVisibility() +{ + D_Covariance::iterator it_cov = covariances_.begin(); + D_Covariance::iterator end_cov = covariances_.end(); + for ( ; it_cov != end_cov; ++it_cov ) + updateVisibility(*it_cov); +} + +void CovarianceProperty::updateVisibility(const CovarianceVisualPtr& visual) +{ + bool show_covariance = getBool(); + if( !show_covariance ) + { + visual->setVisible( false ); + } + else + { + bool show_position_covariance = position_property_->getBool();; + bool show_orientation_covariance = orientation_property_->getBool(); + visual->setPositionVisible( show_position_covariance ); + visual->setOrientationVisible( show_orientation_covariance ); + } +} + +void CovarianceProperty::updateOrientationFrame() +{ + D_Covariance::iterator it_cov = covariances_.begin(); + D_Covariance::iterator end_cov = covariances_.end(); + for ( ; it_cov != end_cov; ++it_cov ) + updateOrientationFrame(*it_cov); +} + +void CovarianceProperty::updateOrientationFrame(const CovarianceVisualPtr& visual) +{ + bool use_rotating_frame = ( orientation_frame_property_->getOptionInt() == Local ); + visual->setRotatingFrame( use_rotating_frame ); +} + +void CovarianceProperty::popFrontVisual() +{ + covariances_.pop_front(); +} + +void CovarianceProperty::clearVisual() +{ + covariances_.clear(); +} + +size_t CovarianceProperty::sizeVisual() +{ + return covariances_.size(); +} + +CovarianceProperty::CovarianceVisualPtr CovarianceProperty::createAndPushBackVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) +{ + bool use_rotating_frame = ( orientation_frame_property_->getOptionInt() == Local ); + CovarianceVisualPtr visual(new CovarianceVisual(scene_manager, parent_node, use_rotating_frame) ); + updateVisibility(visual); + updateOrientationFrame(visual); + updateColorAndAlphaAndScale(visual); + covariances_.push_back(visual); + return visual; +} + +bool CovarianceProperty::getPositionBool() +{ + return position_property_->getBool(); +} + +bool CovarianceProperty::getOrientationBool() +{ + return orientation_property_->getBool(); +} + + + +} // end namespace rviz_plugin_covariance diff --git a/src/rviz/default_plugin/covariance_property.h b/src/rviz/default_plugin/covariance_property.h new file mode 100644 index 0000000000..f6147ab973 --- /dev/null +++ b/src/rviz/default_plugin/covariance_property.h @@ -0,0 +1,121 @@ +/* + * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef COVARIANCE_PROPERTY_H +#define COVARIANCE_PROPERTY_H + +#include + +#include + +#include "rviz/properties/bool_property.h" + +namespace Ogre +{ + class SceneManager; + class SceneNode; +} + +namespace rviz +{ + +class Property; +class ColorProperty; +class FloatProperty; +class EnumProperty; +class CovarianceVisual; + +/** @brief Property specialized to provide getter for booleans. */ +class CovarianceProperty: public BoolProperty +{ +Q_OBJECT +public: + typedef boost::shared_ptr CovarianceVisualPtr; + + enum Frame + { + Local, + Fixed, + }; + + enum ColorStyle + { + Unique, + RGB, + }; + + CovarianceProperty( const QString& name = "Covariance", + bool default_value = false, + const QString& description = QString(), + Property* parent = 0, + const char *changed_slot = 0, + QObject* receiver = 0 ); + + virtual ~CovarianceProperty(); + + bool getPositionBool(); + bool getOrientationBool(); + + // Methods to manage the deque of Covariance Visuals + CovarianceVisualPtr createAndPushBackVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node); + void popFrontVisual(); + void clearVisual(); + size_t sizeVisual(); + +public Q_SLOTS: + void updateVisibility(); + +private Q_SLOTS: + void updateColorAndAlphaAndScale(); + void updateOrientationFrame(); + void updateColorStyleChoice(); + +private: + void updateColorAndAlphaAndScale( const CovarianceVisualPtr& visual ); + void updateOrientationFrame( const CovarianceVisualPtr& visual ); + void updateVisibility( const CovarianceVisualPtr& visual ); + + 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_; +}; + +} // end namespace rviz + +#endif // COVARIANCE_PROPERTY_H diff --git a/src/rviz/default_plugin/covariance_visual.cpp b/src/rviz/default_plugin/covariance_visual.cpp new file mode 100644 index 0000000000..fb99c4d466 --- /dev/null +++ b/src/rviz/default_plugin/covariance_visual.cpp @@ -0,0 +1,543 @@ +/* + * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "covariance_visual.h" + +#include + +#include +#include +#include +#include + +#include + +#include + +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) +{ + // 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() +{ + 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() ); +} + +// Local function to force the axis to be right handed for 3D. Taken from ecl_statistics +void makeRightHanded( Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues) +{ + // Note that sorting of eigenvalues may end up with left-hand coordinate system. + // So here we correctly sort it so that it does end up being righ-handed and normalised. + Eigen::Vector3d c0 = eigenvectors.block<3,1>(0,0); c0.normalize(); + Eigen::Vector3d c1 = eigenvectors.block<3,1>(0,1); c1.normalize(); + Eigen::Vector3d c2 = eigenvectors.block<3,1>(0,2); c2.normalize(); + Eigen::Vector3d cc = c0.cross(c1); + if (cc.dot(c2) < 0) { + eigenvectors << c1, c0, c2; + double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e; + } else { + eigenvectors << c0, c1, c2; + } +} + +// Local function to force the axis to be right handed for 2D. Based on the one from ecl_statistics +void makeRightHanded( Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues) +{ + // Note that sorting of eigenvalues may end up with left-hand coordinate system. + // So here we correctly sort it so that it does end up being righ-handed and normalised. + Eigen::Vector3d c0; c0.setZero(); c0.head<2>() = eigenvectors.col(0); c0.normalize(); + Eigen::Vector3d c1; c1.setZero(); c1.head<2>() = eigenvectors.col(1); c1.normalize(); + Eigen::Vector3d cc = c0.cross(c1); + if (cc[2] < 0) { + eigenvectors << c1.head<2>(), c0.head<2>(); + double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e; + } else { + eigenvectors << c0.head<2>(), c1.head<2>(); + } +} + +void computeShapeScaleAndOrientation3D(const Eigen::Matrix3d& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation) +{ + Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity()); + Eigen::Matrix3d eigenvectors(Eigen::Matrix3d::Zero()); + + // NOTE: The SelfAdjointEigenSolver only references the lower triangular part of the covariance matrix + // FIXME: Should we use Eigen's pseudoEigenvectors() ? + Eigen::SelfAdjointEigenSolver eigensolver(covariance); + // Compute eigenvectors and eigenvalues + if (eigensolver.info () == Eigen::Success) + { + eigenvalues = eigensolver.eigenvalues(); + eigenvectors = eigensolver.eigenvectors(); + } + else + { + ROS_WARN_THROTTLE(1, "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); + eigenvalues = Eigen::Vector3d::Zero(); // Setting the scale to zero will hide it on the screen + eigenvectors = Eigen::Matrix3d::Identity(); + } + + // Be sure we have a right-handed orientation system + makeRightHanded(eigenvectors, eigenvalues); + + // Define the rotation + orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), eigenvectors(0,1), eigenvectors(0,2), + eigenvectors(1,0), eigenvectors(1,1), eigenvectors(1,2), + eigenvectors(2,0), eigenvectors(2,1), eigenvectors(2,2))); + + // Define the scale. eigenvalues are the variances, so we take the sqrt to draw the standard deviation + scale.x = 2*std::sqrt (eigenvalues[0]); + scale.y = 2*std::sqrt (eigenvalues[1]); + scale.z = 2*std::sqrt (eigenvalues[2]); +} + +enum Plane { + YZ_PLANE, // normal is x-axis + XZ_PLANE, // normal is y-axis + XY_PLANE // normal is z-axis +}; + +void computeShapeScaleAndOrientation2D(const Eigen::Matrix2d& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation, Plane plane = XY_PLANE) +{ + Eigen::Vector2d eigenvalues(Eigen::Vector2d::Identity()); + Eigen::Matrix2d eigenvectors(Eigen::Matrix2d::Zero()); + + // NOTE: The SelfAdjointEigenSolver only references the lower triangular part of the covariance matrix + // FIXME: Should we use Eigen's pseudoEigenvectors() ? + Eigen::SelfAdjointEigenSolver eigensolver(covariance); + // Compute eigenvectors and eigenvalues + if (eigensolver.info () == Eigen::Success) + { + eigenvalues = eigensolver.eigenvalues(); + eigenvectors = eigensolver.eigenvectors(); + } + else + { + ROS_WARN_THROTTLE(1, "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); + eigenvalues = Eigen::Vector2d::Zero(); // Setting the scale to zero will hide it on the screen + eigenvectors = Eigen::Matrix2d::Identity(); + } + + // Be sure we have a right-handed orientation system + makeRightHanded(eigenvectors, eigenvalues); + + // Define the rotation and scale of the plane + // The Eigenvalues are the variances. The scales are two times the standard + // deviation. The scale of the missing dimension is set to zero. + if(plane == YZ_PLANE) + { + orientation.FromRotationMatrix(Ogre::Matrix3(1, 0, 0, + 0, eigenvectors(0,0), eigenvectors(0,1), + 0, eigenvectors(1,0), eigenvectors(1,1))); + + scale.x = 0; + scale.y = 2*std::sqrt (eigenvalues[0]); + scale.z = 2*std::sqrt (eigenvalues[1]); + + } + else if(plane == XZ_PLANE) + { + orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), 0, eigenvectors(0,1), + 0, 1, 0, + eigenvectors(1,0), 0, eigenvectors(1,1))); + + scale.x = 2*std::sqrt (eigenvalues[0]); + scale.y = 0; + scale.z = 2*std::sqrt (eigenvalues[1]); + } + else // plane == XY_PLANE + { + orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), eigenvectors(0,1), 0, + eigenvectors(1,0), eigenvectors(1,1), 0, + 0, 0, 1)); + + scale.x = 2*std::sqrt (eigenvalues[0]); + scale.y = 2*std::sqrt (eigenvalues[1]); + scale.z = 0; + } +} + +void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale) +{ + radian_scale /= 2.0; + if(radian_scale > deg2rad(85.0)) radian_scale = deg2rad(85.0); + radian_scale = 2.0 * tan(radian_scale); +} + +void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& pose ) +{ + // check for NaN in covariance + for (unsigned i = 0; i < 3; ++i) + { + if(isnan(pose.covariance[i])) + { + ROS_WARN_THROTTLE(1, "covariance contains NaN"); + return; + } + } + + if(pose.covariance[14] <= 0 && pose.covariance[21] <= 0 && pose.covariance[28] <= 0 ) + pose_2d_ = true; + else + pose_2d_ = false; + + updateOrientationVisibility(); + + // 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 + // inverse of pose's orientation. + fixed_orientation_node_->setOrientation(ori.Inverse()); + // Map covariance to a Eigen::Matrix + Eigen::Map > covariance(pose.covariance.data()); + + updatePosition(covariance); + if(!pose_2d_) + { + updateOrientation(covariance, kRoll); + updateOrientation(covariance, kPitch); + updateOrientation(covariance, kYaw); + } + else + { + updateOrientation(covariance, kYaw2D); + } + +} + +void CovarianceVisual::updatePosition( const Eigen::Matrix6d& covariance ) +{ + // Compute shape and orientation for the position part of covariance + Ogre::Vector3 shape_scale; + Ogre::Quaternion shape_orientation; + if(pose_2d_) + { + 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; + } + else + { + computeShapeScaleAndOrientation3D(covariance.topLeftCorner<3,3>(), shape_scale, shape_orientation); + } + // Rotate and scale the position scene node + position_node_->setOrientation(shape_orientation); + if(!shape_scale.isNaN()) + position_node_->setScale(shape_scale); + else + ROS_WARN_STREAM("position shape_scale contains NaN: " << shape_scale); +} + +void CovarianceVisual::updateOrientation( const Eigen::Matrix6d& covariance, ShapeIndex index ) +{ + Ogre::Vector3 shape_scale; + Ogre::Quaternion shape_orientation; + // Compute shape and orientation for the orientation shape + if(pose_2d_) + { + // We should only enter on this scope if the index is kYaw2D + 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; + } + else + { + assert(index != kYaw2D); + + // Get the correct sub-matrix based on the index + Eigen::Matrix2d covarianceAxis; + if(index == kRoll) + { + covarianceAxis = covariance.bottomRightCorner<2,2>(); + } + else if(index == kPitch) + { + covarianceAxis << covariance(3,3), covariance(3,5), covariance(5,3), covariance(5,5); + } + else if(index == kYaw) + { + covarianceAxis = covariance.block<2,2>(3,3); + } + + // 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); + // 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; + } + + // Rotate and scale the scene node of the orientation part + orientation_node_[index]->setOrientation(shape_orientation); + if(!shape_scale.isNaN()) + orientation_node_[index]->setScale(shape_scale); + else + ROS_WARN_STREAM("orientation shape_scale contains NaN: " << shape_scale); +} + +void CovarianceVisual::setScales( float pos_scale, float ori_scale) +{ + setPositionScale(pos_scale); + setOrientationScale(ori_scale); +} + +void CovarianceVisual::setPositionScale( float pos_scale ) +{ + if(pose_2d_) + position_scale_node_->setScale( pos_scale, pos_scale, 1.0 ); + else + position_scale_node_->setScale( pos_scale, pos_scale, pos_scale ); +} + +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 + for(int i = 0; i < kNumOriShapes; 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 ); + } + else + { + // For the cylinder in 3D case is the y-axis + orientation_offset_node_[i]->setScale( ori_scale, 1.0, ori_scale ); + } + } +} + +void CovarianceVisual::setPositionColor(const Ogre::ColourValue& c) +{ + position_shape_->setColor(c); +} + +void CovarianceVisual::setOrientationColor(const Ogre::ColourValue& c) +{ + for(int i = 0; i < kNumOriShapes; i++) + { + orientation_shape_[i]->setColor(c); + } +} + +void CovarianceVisual::setOrientationColorToRGB( float a ) +{ + orientation_shape_[kRoll]->setColor(Ogre::ColourValue(1.0, 0.0, 0.0, a )); + orientation_shape_[kPitch]->setColor(Ogre::ColourValue(0.0, 1.0, 0.0, a )); + orientation_shape_[kYaw]->setColor(Ogre::ColourValue(0.0, 0.0, 1.0, a )); + orientation_shape_[kYaw2D]->setColor(Ogre::ColourValue(0.0, 0.0, 1.0, a )); +} + +void CovarianceVisual::setPositionColor( float r, float g, float b, float a ) +{ + setPositionColor( Ogre::ColourValue(r, g, b, a )); +} + +void CovarianceVisual::setOrientationColor( float r, float g, float b, float a ) +{ + setOrientationColor( Ogre::ColourValue(r, g, b, a )); +} + +const Ogre::Vector3& CovarianceVisual::getPositionCovarianceScale() +{ + return position_node_->getScale(); +} + +const Ogre::Quaternion& CovarianceVisual::getPositionCovarianceOrientation() +{ + return position_node_->getOrientation(); +} + +void CovarianceVisual::setUserData( const Ogre::Any& data ) +{ + position_shape_->setUserData( data ); + for(int i = 0; i < kNumOriShapes; i++) + { + orientation_shape_[i]->setUserData( data ); + } +} + +void CovarianceVisual::setVisible( bool visible ) +{ + setPositionVisible( visible ); + setOrientationVisible( visible ); +} + +void CovarianceVisual::setPositionVisible( bool visible ) +{ + position_node_->setVisible( visible ); +} + +void CovarianceVisual::setOrientationVisible( bool visible ) +{ + orientation_visible_ = visible; + updateOrientationVisibility(); +} + +void CovarianceVisual::updateOrientationVisibility() +{ + orientation_offset_node_[kRoll]->setVisible( orientation_visible_ && !pose_2d_); + orientation_offset_node_[kPitch]->setVisible( orientation_visible_ && !pose_2d_); + orientation_offset_node_[kYaw]->setVisible( orientation_visible_ && !pose_2d_); + orientation_offset_node_[kYaw2D]->setVisible( orientation_visible_ && pose_2d_); +} + + +const Ogre::Vector3& CovarianceVisual::getPosition() +{ + return position_node_->getPosition(); +} + +const Ogre::Quaternion& CovarianceVisual::getOrientation() +{ + return position_node_->getOrientation(); +} + +void CovarianceVisual::setPosition( const Ogre::Vector3& position ) +{ + root_node_->setPosition( position ); +} + +void CovarianceVisual::setOrientation( const Ogre::Quaternion& orientation ) +{ + root_node_->setOrientation( orientation ); +} + +void CovarianceVisual::setRotatingFrame( bool is_local_rotation ) +{ + if(local_rotation_ == is_local_rotation) + return; + + local_rotation_ = is_local_rotation; + + if(local_rotation_) + root_node_->addChild( fixed_orientation_node_->removeChild( orientation_scale_node_->getName() ) ); + else + fixed_orientation_node_->addChild( root_node_->removeChild( orientation_scale_node_->getName() ) ); + +} + +Shape* CovarianceVisual::getOrientationShape(ShapeIndex index) +{ + return orientation_shape_[index]; +} + +} // namespace rviz diff --git a/src/rviz/default_plugin/covariance_visual.h b/src/rviz/default_plugin/covariance_visual.h new file mode 100644 index 0000000000..da56fbc23b --- /dev/null +++ b/src/rviz/default_plugin/covariance_visual.h @@ -0,0 +1,236 @@ +/* + * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef COVARIANCE_VISUAL_H +#define COVARIANCE_VISUAL_H + +#include + +#include "rviz/ogre_helpers/object.h" + +#include + +#include + +#include + +#include + +using std::isnan; + +namespace Ogre +{ +class SceneManager; +class SceneNode; +class Any; +} + +namespace Eigen +{ + typedef Matrix Matrix6d; +} + +namespace rviz +{ +class Shape; +class CovarianceProperty; + +/** + * \class CovarianceVisual + * \brief CovarianceVisual consisting in a ellipse for position and 2D ellipses along the axis for orientation. + */ +class CovarianceVisual : public Object +{ +public: + enum ShapeIndex + { + kRoll=0, + kPitch=1, + kYaw=2, + kYaw2D=3, + kNumOriShapes + }; + +private: + /** + * \brief Private Constructor + * + * CovarianceVisual can only be constructed by friend class CovarianceProperty. + * + * @param scene_manager The scene manager to use to construct any necessary objects + * @param parent_object A rviz object that this covariance will be attached. + * @param is_local_rotation Initial attachment of the rotation part + * @param is_visible Initial visibility + * @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); +public: + virtual ~CovarianceVisual(); + + /** + * \brief Set the position and orientation scales for this covariance + * + * @param pos_scale Scale of the position covariance + * @param ori_scale Scale of the orientation covariance + */ + void setScales( float pos_scale, float ori_scale); + void setPositionScale( float pos_scale ); + void setOrientationScale( float ori_scale ); + + /** + * \brief Set the color of the position covariance. Values are in the range [0, 1] + * + * @param r Red component + * @param g Green component + * @param b Blue component + */ + virtual void setPositionColor( float r, float g, float b, float a ); + void setPositionColor(const Ogre::ColourValue& color); + + /** + * \brief Set the color of the orientation covariance. Values are in the range [0, 1] + * + * @param r Red component + * @param g Green component + * @param b Blue component + */ + virtual void setOrientationColor( float r, float g, float b, float a ); + void setOrientationColor(const Ogre::ColourValue& color); + void setOrientationColorToRGB(float a); + + /** @brief Set the covariance. + * + * This effectively changes the orientation and scale of position and orientation + * covariance shapes + */ + virtual void setCovariance( const geometry_msgs::PoseWithCovariance& pose ); + + virtual const Ogre::Vector3& getPositionCovarianceScale(); + virtual const Ogre::Quaternion& getPositionCovarianceOrientation(); + + /** + * \brief Get the root scene node of the position part of this covariance + * @return the root scene node of the position part of this covariance + */ + Ogre::SceneNode* getPositionSceneNode() { return position_scale_node_; } + + /** + * \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_; } + + /** + * \brief Get the shape used to display position covariance + * @return the shape used to display position covariance + */ + 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); + + /** + * \brief Sets user data on all ogre objects we own + */ + virtual void setUserData( const Ogre::Any& data ); + + /** + * \brief Sets visibility of this covariance + * + * Convenience method that sets visibility of both position and orientation parts. + */ + virtual void setVisible( bool visible ); + + /** + * \brief Sets visibility of the position part of this covariance + */ + virtual void setPositionVisible( bool visible ); + + /** + * \brief Sets visibility of the orientation part of this covariance + */ + virtual void setOrientationVisible( bool visible ); + + /** + * \brief Sets position of the frame this covariance is attached + */ + virtual void setPosition( const Ogre::Vector3& position ); + + /** + * \brief Sets orientation of the frame this covariance is attached + */ + virtual void setOrientation( const Ogre::Quaternion& orientation ); + + /** + * \brief Sets which frame to attach the covariance of the orientation + */ + virtual void setRotatingFrame( bool use_rotating_frame ); + +private: + void updatePosition( const Eigen::Matrix6d& covariance ); + void updateOrientation( const Eigen::Matrix6d& covariance, ShapeIndex index ); + void updateOrientationVisibility(); + + Ogre::SceneNode* root_node_; + Ogre::SceneNode* fixed_orientation_node_; + Ogre::SceneNode* position_scale_node_; + Ogre::SceneNode* position_node_; + + Ogre::SceneNode* orientation_scale_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 + + bool local_rotation_; + + bool pose_2d_; + + bool orientation_visible_; ///< If the orientation component is visible. + +private: + // Hide Object methods we don't want to expose + // NOTE: Apparently we still need to define them... + virtual void setScale( const Ogre::Vector3& scale ) {}; + virtual void setColor( float r, float g, float b, float a ) {}; + virtual const Ogre::Vector3& getPosition(); + virtual const Ogre::Quaternion& getOrientation(); + + // Make CovarianceProperty friend class so it create CovarianceVisual objects + friend class CovarianceProperty; +}; + +} // namespace rviz + +#endif /* COVARIANCE_VISUAL_H */ diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index 9128369c2b..ce7f388c1f 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/src/rviz/default_plugin/odometry_display.cpp @@ -27,37 +27,26 @@ * POSSIBILITY OF SUCH DAMAGE. */ - -#include - -#include - -#include "rviz/frame_manager.h" #include "rviz/ogre_helpers/arrow.h" +#include "rviz/ogre_helpers/axes.h" +#include "rviz/properties/enum_property.h" #include "rviz/properties/color_property.h" #include "rviz/properties/float_property.h" #include "rviz/properties/int_property.h" -#include "rviz/properties/ros_topic_property.h" #include "rviz/validate_floats.h" -#include "rviz/display_context.h" + +#include +#include #include "odometry_display.h" +#include "covariance_property.h" +#include "covariance_visual.h" namespace rviz { OdometryDisplay::OdometryDisplay() - : Display() - , messages_received_(0) { - topic_property_ = new RosTopicProperty( "Topic", "", - QString::fromStdString( ros::message_traits::datatype() ), - "nav_msgs::Odometry topic to subscribe to.", - this, SLOT( updateTopic() )); - - color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), - "Color of the arrows.", - this, SLOT( updateColor() )); position_tolerance_property_ = new FloatProperty( "Position Tolerance", .1, "Distance, in meters from the last arrow dropped, " @@ -76,29 +65,63 @@ OdometryDisplay::OdometryDisplay() this ); keep_property_->setMin( 0 ); - length_property_ = new FloatProperty( "Length", 1.0, - "Length of each arrow.", - this, SLOT( updateLength() )); + 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 ); + + color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), + "Color of the arrows.", + shape_property_, SLOT( updateColorAndAlpha() ), this); + + alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the arrow.", + shape_property_, SLOT( updateColorAndAlpha() ), this); + alpha_property_->setMin( 0 ); + alpha_property_->setMax( 1 ); + + shaft_length_property_ = new FloatProperty( "Shaft Length", 1, "Length of the each arrow's shaft, in meters.", + shape_property_, SLOT( updateArrowsGeometry() ), this); + + // 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); + + // aleeper: default changed from 0.2 to match change in arrow.cpp + head_radius_property_ = new FloatProperty( "Head Radius", 0.1, "Radius of the each arrow's head, in meters.", + shape_property_, SLOT( updateArrowsGeometry() ), this); + + axes_length_property_ = new FloatProperty( "Axes Length", 1, "Length of each axis, in meters.", + shape_property_, SLOT( updateAxisGeometry() ), this); + + axes_radius_property_ = new FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.", + shape_property_, SLOT( updateAxisGeometry() ), this); + + covariance_property_ = new CovarianceProperty( "Covariance", true, "Whether or not the covariances of the messages should be shown.", + this, SLOT( queueRender() )); + } OdometryDisplay::~OdometryDisplay() { if ( initialized() ) { - unsubscribe(); clear(); - delete tf_filter_; } } void OdometryDisplay::onInitialize() { - tf_filter_ = new tf::MessageFilter( *context_->getTFClient(), fixed_frame_.toStdString(), - 5, update_nh_ ); + MFDClass::onInitialize(); + updateShapeChoice(); +} - tf_filter_->connectInput( sub_ ); - tf_filter_->registerCallback( boost::bind( &OdometryDisplay::incomingMessage, this, _1 )); - context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this ); +void OdometryDisplay::onEnable() +{ + MFDClass::onEnable(); + updateShapeVisibility(); } void OdometryDisplay::clear() @@ -111,101 +134,128 @@ void OdometryDisplay::clear() } arrows_.clear(); + // covariances are stored in covariance_property_ + covariance_property_->clearVisual(); + + D_Axes::iterator it_axes = axes_.begin(); + D_Axes::iterator end_axes = axes_.end(); + for ( ; it_axes != end_axes; ++it_axes ) + { + delete *it_axes; + } + axes_.clear(); + if( last_used_message_ ) { last_used_message_.reset(); } - - tf_filter_->clear(); - - messages_received_ = 0; - setStatus( StatusProperty::Warn, "Topic", "No messages received" ); -} - -void OdometryDisplay::updateTopic() -{ - unsubscribe(); - clear(); - subscribe(); - context_->queueRender(); } -void OdometryDisplay::updateColor() +void OdometryDisplay::updateColorAndAlpha() { QColor color = color_property_->getColor(); float red = color.redF(); float green = color.greenF(); float blue = color.blueF(); + float alpha = alpha_property_->getFloat(); D_Arrow::iterator it = arrows_.begin(); D_Arrow::iterator end = arrows_.end(); for( ; it != end; ++it ) { Arrow* arrow = *it; - arrow->setColor( red, green, blue, 1.0f ); + arrow->setColor( red, green, blue, alpha ); } context_->queueRender(); } -void OdometryDisplay::updateLength() +void OdometryDisplay::updateArrowsGeometry() { - float length = length_property_->getFloat(); D_Arrow::iterator it = arrows_.begin(); D_Arrow::iterator end = arrows_.end(); - Ogre::Vector3 scale( length, length, length ); for ( ; it != end; ++it ) { - Arrow* arrow = *it; - arrow->setScale( scale ); + updateGeometry(*it); } context_->queueRender(); } -void OdometryDisplay::subscribe() +void OdometryDisplay::updateAxisGeometry() { - if ( !isEnabled() ) + D_Axes::iterator it = axes_.begin(); + D_Axes::iterator end = axes_.end(); + for ( ; it != end; ++it ) { - return; + updateGeometry(*it); } + context_->queueRender(); +} - try - { - sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 5 ); - setStatus( StatusProperty::Ok, "Topic", "OK" ); - } - catch( ros::Exception& e ) - { - setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); - } +void OdometryDisplay::updateGeometry( Axes* axes ) +{ + axes->set( axes_length_property_->getFloat(), + axes_radius_property_->getFloat() ); } -void OdometryDisplay::unsubscribe() +void OdometryDisplay::updateGeometry( Arrow* arrow ) { - sub_.unsubscribe(); + arrow->set( shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat() ); } -void OdometryDisplay::onEnable() +void OdometryDisplay::updateShapeChoice() { - subscribe(); + bool use_arrow = ( shape_property_->getOptionInt() == ArrowShape ); + + color_property_->setHidden( !use_arrow ); + alpha_property_->setHidden( !use_arrow ); + shaft_length_property_->setHidden( !use_arrow ); + shaft_radius_property_->setHidden( !use_arrow ); + head_length_property_->setHidden( !use_arrow ); + head_radius_property_->setHidden( !use_arrow ); + + axes_length_property_->setHidden( use_arrow ); + axes_radius_property_->setHidden( use_arrow ); + + updateShapeVisibility(); + + context_->queueRender(); } -void OdometryDisplay::onDisable() +void OdometryDisplay::updateShapeVisibility() { - unsubscribe(); - clear(); + bool use_arrow = (shape_property_->getOptionInt() == ArrowShape); + + D_Arrow::iterator it = arrows_.begin(); + D_Arrow::iterator end = arrows_.end(); + for ( ; it != end; ++it ) + { + (*it)->getSceneNode()->setVisible( use_arrow ); + } + + D_Axes::iterator it_axes = axes_.begin(); + D_Axes::iterator end_axes = axes_.end(); + for ( ; it_axes != end_axes; ++it_axes ) + { + (*it_axes)->getSceneNode()->setVisible( !use_arrow ); + } } 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 ) return valid; } -void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message ) +void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& message ) { - ++messages_received_; + typedef CovarianceProperty::CovarianceVisualPtr CovarianceVisualPtr; if( !validateFloats( *message )) { @@ -213,8 +263,6 @@ void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& messa return; } - setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" ); - if( last_used_message_ ) { 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); @@ -222,6 +270,7 @@ void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& messa 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); + // 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() ) { @@ -229,44 +278,59 @@ void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& messa } } - Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f ); - - transformArrow( message, arrow ); - - QColor color = color_property_->getColor(); - arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f ); - - float length = length_property_->getFloat(); - Ogre::Vector3 scale( length, length, length ); - arrow->setScale( scale ); - - arrows_.push_back( arrow ); - - last_used_message_ = message; - context_->queueRender(); -} - -void OdometryDisplay::transformArrow( const nav_msgs::Odometry::ConstPtr& message, Arrow* arrow ) -{ Ogre::Vector3 position; Ogre::Quaternion orientation; if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation )) { ROS_ERROR( "Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable( getName() ), message->header.frame_id.c_str(), qPrintable( fixed_frame_ )); + return; } - arrow->setPosition( position ); + // If we arrive here, we're good. Continue... + + // Create a scene node, and attach the arrow and the covariance to it + Axes* axes = new Axes( scene_manager_, scene_node_, + axes_length_property_->getFloat(), + axes_radius_property_->getFloat() ); + Arrow* arrow = new Arrow( scene_manager_, scene_node_, + shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat() ); + CovarianceVisualPtr cov = covariance_property_->createAndPushBackVisual(scene_manager_, scene_node_ ); + + // Position the axes + axes->setPosition( position ); + axes->setOrientation( orientation ); - // Arrow points in -Z direction, so rotate the orientation before display. - // TODO: is it safe to change Arrow to point in +X direction? + // Position the arrow. Remember the arrow points in -Z direction, so rotate the orientation before display. + arrow->setPosition( position ); arrow->setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y )); -} -void OdometryDisplay::fixedFrameChanged() -{ - tf_filter_->setTargetFrame( fixed_frame_.toStdString() ); - clear(); + // Position the frame where the covariance is attached covariance + cov->setPosition( position ); + cov->setOrientation( orientation ); + + // Set up arrow color + QColor color = color_property_->getColor(); + float alpha = alpha_property_->getFloat(); + arrow->setColor( color.redF(), color.greenF(), color.blueF(), alpha); + + // Set up the covariance based on the message + cov->setCovariance(message->pose); + + // Show/Hide things based on current properties + bool use_arrow = (shape_property_->getOptionInt() == ArrowShape); + arrow->getSceneNode()->setVisible( use_arrow ); + axes->getSceneNode()->setVisible( !use_arrow ); + + // store everything + axes_.push_back( axes ); + arrows_.push_back( arrow ); + + last_used_message_ = message; + context_->queueRender(); } void OdometryDisplay::update( float wall_dt, float ros_dt ) @@ -278,21 +342,26 @@ void OdometryDisplay::update( float wall_dt, float ros_dt ) { delete arrows_.front(); arrows_.pop_front(); + + // covariance visuals are stored into covariance_property_ + covariance_property_->popFrontVisual(); + + delete axes_.front(); + axes_.pop_front(); } } + + assert(arrows_.size() == axes_.size()); + assert(axes_.size() == covariance_property_->sizeVisual()); + } void OdometryDisplay::reset() { - Display::reset(); + MFDClass::reset(); clear(); } -void OdometryDisplay::setTopic( const QString &topic, const QString &datatype ) -{ - topic_property_->setString( topic ); -} - } // namespace rviz #include diff --git a/src/rviz/default_plugin/odometry_display.h b/src/rviz/default_plugin/odometry_display.h index 08f37815d8..f7a18d4ed5 100644 --- a/src/rviz/default_plugin/odometry_display.h +++ b/src/rviz/default_plugin/odometry_display.h @@ -41,71 +41,86 @@ #include #endif +#include "rviz/message_filter_display.h" #include -#include "rviz/display.h" - namespace rviz { - class Arrow; +class Axes; class ColorProperty; class FloatProperty; class IntProperty; -class RosTopicProperty; +class EnumProperty; + +class CovarianceProperty; /** * \class OdometryDisplay * \brief Accumulates and displays the pose from a nav_msgs::Odometry message */ -class OdometryDisplay: public Display +class OdometryDisplay: public MessageFilterDisplay { Q_OBJECT public: + enum Shape + { + ArrowShape, + AxesShape, + }; + OdometryDisplay(); virtual ~OdometryDisplay(); - // Overrides from Display + // Overides of MessageFilterDisplay virtual void onInitialize(); - virtual void fixedFrameChanged(); - virtual void update( float wall_dt, float ros_dt ); virtual void reset(); - - virtual void setTopic( const QString &topic, const QString &datatype ); + // Overides of Display + virtual void update( float wall_dt, float ros_dt ); protected: - // overrides from Display + /** @brief Overridden from MessageFilterDisplay to get Arrow/Axes visibility correct. */ virtual void onEnable(); - virtual void onDisable(); private Q_SLOTS: - void updateColor(); - void updateTopic(); - void updateLength(); + void updateShapeChoice(); + void updateShapeVisibility(); + void updateColorAndAlpha(); + void updateArrowsGeometry(); + void updateAxisGeometry(); private: - void subscribe(); - void unsubscribe(); + void updateGeometry( Arrow* arrow ); + void updateGeometry( Axes* axes ); void clear(); - void incomingMessage( const nav_msgs::Odometry::ConstPtr& message ); - void transformArrow( const nav_msgs::Odometry::ConstPtr& message, Arrow* arrow ); + virtual void processMessage( const nav_msgs::Odometry::ConstPtr& message ); typedef std::deque D_Arrow; - D_Arrow arrows_; + typedef std::deque D_Axes; - uint32_t messages_received_; + D_Arrow arrows_; + D_Axes axes_; nav_msgs::Odometry::ConstPtr last_used_message_; - message_filters::Subscriber sub_; - tf::MessageFilter* tf_filter_; + + EnumProperty* shape_property_; ColorProperty* color_property_; - RosTopicProperty* topic_property_; + FloatProperty* alpha_property_; FloatProperty* position_tolerance_property_; FloatProperty* angle_tolerance_property_; IntProperty* keep_property_; - FloatProperty* length_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_; + + CovarianceProperty* covariance_property_; }; } // namespace rviz diff --git a/src/rviz/default_plugin/pose_with_covariance_display.cpp b/src/rviz/default_plugin/pose_with_covariance_display.cpp new file mode 100644 index 0000000000..cdb84f1650 --- /dev/null +++ b/src/rviz/default_plugin/pose_with_covariance_display.cpp @@ -0,0 +1,345 @@ +/* + * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "rviz/display_context.h" +#include "rviz/frame_manager.h" +#include "rviz/ogre_helpers/arrow.h" +#include "rviz/ogre_helpers/axes.h" +#include "rviz/ogre_helpers/shape.h" +#include "rviz/properties/color_property.h" +#include "rviz/properties/enum_property.h" +#include "rviz/properties/float_property.h" +#include "rviz/properties/bool_property.h" +#include "rviz/properties/quaternion_property.h" +#include "rviz/properties/string_property.h" +#include "rviz/properties/vector_property.h" +#include "rviz/selection/selection_manager.h" +#include "rviz/validate_floats.h" + +#include "pose_with_covariance_display.h" +#include "covariance_property.h" +#include "covariance_visual.h" + +#include + +namespace rviz +{ + +class PoseWithCovarianceDisplaySelectionHandler: public SelectionHandler +{ +public: + PoseWithCovarianceDisplaySelectionHandler( PoseWithCovarianceDisplay* display, DisplayContext* context ) + : SelectionHandler( context ) + , display_( display ) + {} + + void createProperties( const Picked& obj, Property* parent_property ) + { + Property* cat = new Property( "Pose " + display_->getName(), QVariant(), "", parent_property ); + properties_.push_back( cat ); + + frame_property_ = new StringProperty( "Frame", "", "", cat ); + frame_property_->setReadOnly( true ); + + position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, "", cat ); + position_property_->setReadOnly( true ); + + orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, "", cat ); + orientation_property_->setReadOnly( true ); + + covariance_position_property_ = new VectorProperty( "Covariance Position", Ogre::Vector3::ZERO, "", cat ); + covariance_position_property_->setReadOnly( true ); + + covariance_orientation_property_ = new VectorProperty( "Covariance Orientation", Ogre::Vector3::ZERO, "", cat ); + covariance_orientation_property_->setReadOnly( true ); + } + + void getAABBs( const Picked& obj, V_AABB& aabbs ) + { + if( display_->pose_valid_ ) + { + if( display_->shape_property_->getOptionInt() == PoseWithCovarianceDisplay::ArrowShape ) + { + aabbs.push_back( display_->arrow_->getHead()->getEntity()->getWorldBoundingBox() ); + aabbs.push_back( display_->arrow_->getShaft()->getEntity()->getWorldBoundingBox() ); + } + else + { + aabbs.push_back( display_->axes_->getXShape()->getEntity()->getWorldBoundingBox() ); + aabbs.push_back( display_->axes_->getYShape()->getEntity()->getWorldBoundingBox() ); + aabbs.push_back( display_->axes_->getZShape()->getEntity()->getWorldBoundingBox() ); + } + + if( display_->covariance_property_->getBool() ) + { + if(display_->covariance_property_->getPositionBool()) + { + aabbs.push_back( display_->covariance_->getPositionShape()->getEntity()->getWorldBoundingBox() ); + } + if(display_->covariance_property_->getOrientationBool()) + { + aabbs.push_back( display_->covariance_->getOrientationShape(CovarianceVisual::kRoll)->getEntity()->getWorldBoundingBox() ); + aabbs.push_back( display_->covariance_->getOrientationShape(CovarianceVisual::kPitch)->getEntity()->getWorldBoundingBox() ); + aabbs.push_back( display_->covariance_->getOrientationShape(CovarianceVisual::kYaw)->getEntity()->getWorldBoundingBox() ); + } + } + } + } + + void setMessage(const geometry_msgs::PoseWithCovarianceStampedConstPtr& message) + { + // properties_.size() should only be > 0 after createProperties() + // and before destroyProperties(), during which frame_property_, + // position_property_, and orientation_property_ should be valid + // pointers. + if( properties_.size() > 0 ) + { + frame_property_->setStdString( message->header.frame_id ); + position_property_->setVector( Ogre::Vector3( message->pose.pose.position.x, + message->pose.pose.position.y, + message->pose.pose.position.z )); + orientation_property_->setQuaternion( Ogre::Quaternion( message->pose.pose.orientation.w, + message->pose.pose.orientation.x, + message->pose.pose.orientation.y, + message->pose.pose.orientation.z )); + covariance_position_property_->setVector( Ogre::Vector3( message->pose.covariance[0+0*6], + message->pose.covariance[1+1*6], + message->pose.covariance[2+2*6] )); + + covariance_orientation_property_->setVector( Ogre::Vector3( message->pose.covariance[3+3*6], + message->pose.covariance[4+4*6], + message->pose.covariance[5+5*6] )); + } + } + +private: + PoseWithCovarianceDisplay* display_; + StringProperty* frame_property_; + VectorProperty* position_property_; + QuaternionProperty* orientation_property_; + VectorProperty* covariance_position_property_; + VectorProperty* covariance_orientation_property_; + +}; + +PoseWithCovarianceDisplay::PoseWithCovarianceDisplay() + : pose_valid_( false ) +{ + 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 ); + + color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrow.", + this, SLOT( updateColorAndAlpha() )); + + alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the arrow.", + this, SLOT( updateColorAndAlpha() )); + alpha_property_->setMin( 0 ); + alpha_property_->setMax( 1 ); + + shaft_length_property_ = new FloatProperty( "Shaft Length", 1, "Length of the arrow's shaft, in meters.", + this, SLOT( updateArrowGeometry() )); + + // 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() )); + + // aleeper: default changed from 0.2 to match change in arrow.cpp + head_radius_property_ = new FloatProperty( "Head Radius", 0.1, "Radius of the arrow's head, in meters.", + this, SLOT( updateArrowGeometry() )); + + axes_length_property_ = new FloatProperty( "Axes Length", 1, "Length of each axis, in meters.", + this, SLOT( updateAxisGeometry() )); + + axes_radius_property_ = new FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.", + this, SLOT( updateAxisGeometry() )); + + covariance_property_ = new CovarianceProperty( "Covariance", true, "Whether or not the covariances of the messages should be shown.", + this, SLOT( queueRender() )); +} + +void PoseWithCovarianceDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + arrow_ = new Arrow( scene_manager_, scene_node_, + shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat() ); + // Arrow points in -Z direction, so rotate the orientation before display. + // 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_length_property_->getFloat(), + axes_radius_property_->getFloat() ); + + covariance_ = covariance_property_->createAndPushBackVisual(scene_manager_, scene_node_ ); + + updateShapeChoice(); + updateColorAndAlpha(); + + coll_handler_.reset( new PoseWithCovarianceDisplaySelectionHandler( this, context_ )); + coll_handler_->addTrackedObjects( arrow_->getSceneNode() ); + coll_handler_->addTrackedObjects( axes_->getSceneNode() ); + coll_handler_->addTrackedObjects( covariance_->getPositionSceneNode() ); + coll_handler_->addTrackedObjects( covariance_->getOrientationSceneNode() ); +} + +PoseWithCovarianceDisplay::~PoseWithCovarianceDisplay() +{ + if ( initialized() ) + { + delete arrow_; + delete axes_; + } +} + +void PoseWithCovarianceDisplay::onEnable() +{ + MFDClass::onEnable(); + updateShapeVisibility(); +} + +void PoseWithCovarianceDisplay::updateColorAndAlpha() +{ + Ogre::ColourValue color = color_property_->getOgreColor(); + color.a = alpha_property_->getFloat(); + + arrow_->setColor( color ); + + context_->queueRender(); +} + +void PoseWithCovarianceDisplay::updateArrowGeometry() +{ + arrow_->set( shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat() ); + context_->queueRender(); +} + +void PoseWithCovarianceDisplay::updateAxisGeometry() +{ + axes_->set( axes_length_property_->getFloat(), + axes_radius_property_->getFloat() ); + context_->queueRender(); +} + +void PoseWithCovarianceDisplay::updateShapeChoice() +{ + bool use_arrow = ( shape_property_->getOptionInt() == ArrowShape ); + + color_property_->setHidden( !use_arrow ); + alpha_property_->setHidden( !use_arrow ); + shaft_length_property_->setHidden( !use_arrow ); + shaft_radius_property_->setHidden( !use_arrow ); + head_length_property_->setHidden( !use_arrow ); + head_radius_property_->setHidden( !use_arrow ); + + axes_length_property_->setHidden( use_arrow ); + axes_radius_property_->setHidden( use_arrow ); + + updateShapeVisibility(); + + context_->queueRender(); +} + +void PoseWithCovarianceDisplay::updateShapeVisibility() +{ + if( !pose_valid_ ) + { + arrow_->getSceneNode()->setVisible( false ); + axes_->getSceneNode()->setVisible( false ); + covariance_->setVisible( false ); + } + else + { + bool use_arrow = (shape_property_->getOptionInt() == ArrowShape); + arrow_->getSceneNode()->setVisible( use_arrow ); + axes_->getSceneNode()->setVisible( !use_arrow ); + covariance_property_->updateVisibility(); + } +} + +void PoseWithCovarianceDisplay::processMessage( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& message ) +{ + if( !validateFloats( message->pose.pose ) || !validateFloats( message->pose.covariance )) + { + setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation )) + { + ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'", + qPrintable( getName() ), message->header.frame_id.c_str(), qPrintable( fixed_frame_ )); + return; + } + + pose_valid_ = true; + updateShapeVisibility(); + + axes_->setPosition( position ); + axes_->setOrientation( orientation ); + + arrow_->setPosition( position ); + arrow_->setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ) ); + + covariance_->setPosition( position ); + covariance_->setOrientation( orientation ); + covariance_->setCovariance( message->pose ); + + coll_handler_->setMessage( message ); + + context_->queueRender(); +} + +void PoseWithCovarianceDisplay::reset() +{ + MFDClass::reset(); + pose_valid_ = false; + updateShapeVisibility(); +} + +} // namespace rviz + +#include +PLUGINLIB_EXPORT_CLASS( rviz::PoseWithCovarianceDisplay, rviz::Display ) diff --git a/src/rviz/default_plugin/pose_with_covariance_display.h b/src/rviz/default_plugin/pose_with_covariance_display.h new file mode 100644 index 0000000000..4d9904c563 --- /dev/null +++ b/src/rviz/default_plugin/pose_with_covariance_display.h @@ -0,0 +1,115 @@ +/* + * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef POSE_WITH_COVARIANCE_DISPLAY_H +#define POSE_WITH_COVARIANCE_DISPLAY_H + +#include + +#include + +#include "rviz/message_filter_display.h" +#include "rviz/selection/forwards.h" + +namespace rviz +{ +class Arrow; +class Axes; +class ColorProperty; +class EnumProperty; +class FloatProperty; +class BoolProperty; +class Shape; + +class CovarianceVisual; +class CovarianceProperty; + +class PoseWithCovarianceDisplaySelectionHandler; +typedef boost::shared_ptr PoseWithCovarianceDisplaySelectionHandlerPtr; + +/** @brief Displays the pose from a geometry_msgs::PoseWithCovarianceStamped message. */ +class PoseWithCovarianceDisplay: public MessageFilterDisplay +{ +Q_OBJECT +public: + enum Shape + { + ArrowShape, + AxesShape, + }; + + PoseWithCovarianceDisplay(); + virtual ~PoseWithCovarianceDisplay(); + + virtual void onInitialize(); + virtual void reset(); + +protected: + /** @brief Overridden from MessageFilterDisplay to get Arrow/Axes visibility correct. */ + virtual void onEnable(); + +private Q_SLOTS: + void updateShapeVisibility(); + void updateColorAndAlpha(); + void updateShapeChoice(); + void updateAxisGeometry(); + void updateArrowGeometry(); + +private: + void clear(); + + virtual void processMessage( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& message ); + + Arrow* arrow_; + Axes* axes_; + boost::shared_ptr covariance_; + bool pose_valid_; + PoseWithCovarianceDisplaySelectionHandlerPtr coll_handler_; + + EnumProperty* shape_property_; + + ColorProperty* color_property_; + FloatProperty* alpha_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_; + + CovarianceProperty* covariance_property_; + + friend class PoseWithCovarianceDisplaySelectionHandler; +}; + +} // namespace rviz + +#endif // POSE_WITH_COVARIANCE_DISPLAY_H diff --git a/src/test/send_covariance_msgs.py b/src/test/send_covariance_msgs.py new file mode 100755 index 0000000000..aeedd943cf --- /dev/null +++ b/src/test/send_covariance_msgs.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import roslib; roslib.load_manifest( 'rviz_plugin_covariance' ) +from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped +import rospy +from math import cos, sin, pi +import tf +import tf_conversions + +publisher_cov = rospy.Publisher( 'pose_with_cov', PoseWithCovarianceStamped, queue_size=5 ) +publisher_pose = rospy.Publisher( 'pose', PoseStamped, queue_size=5 ) + +rospy.init_node( 'test_covariance' ) + +br = tf.TransformBroadcaster() +rate = rospy.Rate(100) +# radius = 1 +angle = 0 +# r = 0 +# p = 0 +# y = 0 + +linear_deviation = 0.5; + + +while not rospy.is_shutdown(): + stamp = rospy.Time.now() + + # Define static pose with covariance + pose_with_cov = PoseWithCovarianceStamped() + pose_with_cov.header.frame_id = "/base_link" + pose_with_cov.header.stamp = stamp + + pose_with_cov.pose.pose.position.x = 3 + pose_with_cov.pose.pose.position.y = 3 + pose_with_cov.pose.pose.position.z = 3 + + ori = pose_with_cov.pose.pose.orientation + ori.x, ori.y, ori.z, ori.w = tf.transformations.quaternion_from_euler(pi/2,pi/3,0) + + pose_with_cov.pose.covariance[0] = linear_deviation**2.0 + pose_with_cov.pose.covariance[6+1] = 0.0001 + pose_with_cov.pose.covariance[12+2] = 0.0001 + pose_with_cov.pose.covariance[18+3] = 0.01 + pose_with_cov.pose.covariance[24+4] = 0.01 + pose_with_cov.pose.covariance[30+5] = 0.01 + + # Define a dynamic pose that should move inside the deviation + pose = PoseStamped() + pose.header.frame_id = "/base_link" + pose.header.stamp = stamp + + pose.pose.position.x = pose_with_cov.pose.pose.position.x + linear_deviation*cos( 10 * angle ) + pose.pose.position.y = pose_with_cov.pose.pose.position.y + pose.pose.position.z = pose_with_cov.pose.pose.position.z + + ori = pose.pose.orientation + ori.x, ori.y, ori.z, ori.w = tf.transformations.quaternion_from_euler(pi/2,pi/3,0) + + publisher_cov.publish( pose_with_cov ) + publisher_pose.publish( pose ) + + # br.sendTransform((radius * cos(angle), radius * sin(angle), 0), + # tf.transformations.quaternion_from_euler(r, p, y), + # rospy.Time.now(), + # "base_link", + # "map") + pos, ori = pose_with_cov.pose.pose.position, pose_with_cov.pose.pose.orientation + br.sendTransform((pos.x, pos.y, pos.z), + (ori.x, ori.y, ori.z, ori.w), + stamp, + "pose", + "base_link") + + angle += .0005 + # r = angle + # p = angle + # y = angle + rate.sleep() +