Revert "Simplify MappedCovarianceVisual using rviz 1.13.4"

This reverts commit 675549e.

This allows to use the rviz version shipped with Kinetic, and we only
need to revert this commit when we decide to drop backwards
compatibility with Kinetic or older versions of rviz.

fuse_viz/CMakeLists.txt

@@ -49,6 +49,7 @@ qt5_wrap_cpp(moc_files
 
 set(source_files
   src/mapped_covariance_property.cpp
+  src/mapped_covariance_visual.cpp
   src/pose_2d_stamped_property.cpp
   src/pose_2d_stamped_visual.cpp
   src/relative_pose_2d_stamped_constraint_property.cpp

fuse_viz/include/fuse_viz/mapped_covariance_visual.h

@@ -30,16 +30,31 @@
 #ifndef MAPPED_COVARIANCE_VISUAL_H
 #define MAPPED_COVARIANCE_VISUAL_H
 
-#include <rviz/default_plugin/covariance_visual.h>
+#include <cmath>
+
+#include <rviz/ogre_helpers/object.h>
+
+#include <boost/scoped_ptr.hpp>
+
+#include <geometry_msgs/PoseWithCovariance.h>
+
+#include <Eigen/Dense>
+
+#include <OgreColourValue.h>
+#include <OgreVector3.h>
 
 namespace Ogre
 {
-
 class SceneManager;
 class SceneNode;
-
+class Any;
 }  // namespace Ogre
 
+namespace Eigen
+{
+typedef Matrix<double, 6, 6> Matrix6d;
+}
+
 namespace rviz
 {
 
@@ -53,11 +68,21 @@ class MappedCovarianceProperty;
  * This is mostly a copy of CovarianceVisual from rviz/default_plugin/covariance_visual.h that allows
  * MappedCovarianceProperty be a friend class of MappedCovarianceVisual, so it can call its constructor.
  */
-class MappedCovarianceVisual : public rviz::CovarianceVisual
+class MappedCovarianceVisual : public rviz::Object
 {
 public:
+  enum ShapeIndex
+  {
+    kRoll = 0,
+    kPitch = 1,
+    kYaw = 2,
+    kYaw2D = 3,
+    kNumOriShapes
+  };
+
+private:
   /**
-   * \brief Constructor
+   * \brief Private Constructor
    *
    * MappedCovarianceVisual can only be constructed by friend class MappedCovarianceProperty.
    *
@@ -70,12 +95,158 @@ class MappedCovarianceVisual : public rviz::CovarianceVisual
    */
   MappedCovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation,
                          bool is_visible = true, float pos_scale = 1.0f, float ori_scale = 0.1f,
-                         float ori_offset = 0.1f)
-    : CovarianceVisual(scene_manager, parent_node, is_local_rotation, is_visible, pos_scale, ori_scale, ori_offset)
+                         float ori_offset = 0.1f);
+
+public:
+  ~MappedCovarianceVisual() override;
+
+  /**
+   * \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 setOrientationOffset(float ori_offset);
+  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_root_node_;
   }
 
+  /**
+   * \brief Get the shape used to display position covariance
+   * @return the shape used to display position covariance
+   */
+  rviz::Shape* getPositionShape()
+  {
+    return position_shape_;
+  }
+
+  /**
+   * \brief Get the shape used to display orientation covariance in an especific axis
+   * @return the shape used to display orientation covariance in an especific axis
+   */
+  rviz::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_root_node_;
+  Ogre::SceneNode* orientation_offset_node_[kNumOriShapes];
+
+  rviz::Shape* position_shape_;                    ///< Ellipse used for the position covariance
+  rviz::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.
+
+  Ogre::Vector3 current_ori_scale_[kNumOriShapes];
+  float current_ori_scale_factor_;
+
+  const static float max_degrees;
+
 private:
+  // Hide Object methods we don't want to expose
+  // NOTE: Apparently we still need to define them...
+  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 MappedCovarianceProperty friend class so it create MappedCovarianceVisual objects
   friend class MappedCovarianceProperty;
 };