Enhancement: Add pose covariance displays

plugin_description.xml

@@ -127,6 +127,12 @@
     </description>
     <message_type>geometry_msgs/PoseArray</message_type>
   </class>
+  <class name="rviz/PoseWithCovariance" type="rviz::PoseWithCovarianceDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays a geometry_msgs::PoseWithCovarianceStamped message.  &lt;a href="http://www.ros.org/wiki/rviz/DisplayTypes/PoseWithCovariance"&gt;More Information&lt;/a&gt;.
+    </description>
+    <message_type>geometry_msgs/PoseWithCovarianceStamped</message_type>
+  </class>
   <class name="rviz/Range" type="rviz::RangeDisplay" base_class_type="rviz::Display">
     <description>
       Displays the data from sensor_msgs::Range messages as cones. &lt;a href="http://www.ros.org/wiki/rviz/DisplayTypes/Range"&gt;More Information&lt;/a&gt;

src/rviz/default_plugin/CMakeLists.txt

@@ -3,6 +3,8 @@ include_directories(.)
 qt4_wrap_cpp(MOC_FILES
   axes_display.h
   camera_display.h
+  covariance_visual.h
+  covariance_property.h
   depth_cloud_display.h
   effort_display.h
   fluid_pressure_display.h
@@ -28,6 +30,7 @@ qt4_wrap_cpp(MOC_FILES
   polygon_display.h
   pose_array_display.h
   pose_display.h
+  pose_with_covariance_display.h
   range_display.h
   relative_humidity_display.h
   robot_model_display.h
@@ -48,6 +51,8 @@ qt4_wrap_cpp(MOC_FILES
 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
@@ -86,6 +91,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

src/rviz/default_plugin/covariance_property.cpp

@@ -0,0 +1,237 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * 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 <QColor>
+
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+
+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

src/rviz/default_plugin/covariance_property.h

@@ -0,0 +1,92 @@
+#ifndef COVARIANCE_PROPERTY_H
+#define COVARIANCE_PROPERTY_H
+
+#include <QColor>
+
+#include <OgreColourValue.h>
+
+#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<CovarianceVisual> 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<CovarianceVisualPtr> 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