Draw RelativePose2DStampedConstraint constraints (#107)

* Draw RelativePose2DStampedConstraint constraints
* Dynamically generate display properties for the constraint sources
* Cache the constraint sources properties config so it's applied when
  the properties are later created
* Create Variable visual + property, as we do for the Constraint

fuse_viz/CMakeLists.txt

@@ -2,15 +2,22 @@ cmake_minimum_required(VERSION 2.8.3)
 project(fuse_viz)
 
 set(build_depends
+  fuse_constraints
+  fuse_core
   fuse_msgs
   fuse_variables
+  geometry_msgs
   rviz
+  tf2_geometry_msgs
 )
 
 find_package(catkin REQUIRED COMPONENTS
   ${build_depends}
 )
 
+find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
+
 find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
 set(QT_LIBRARIES Qt5::Widgets)
 
@@ -28,14 +35,25 @@ catkin_package(
   CATKIN_DEPENDS
     ${build_depends}
   DEPENDS
+    Boost
+    EIGEN3
     QT
 )
 
 qt5_wrap_cpp(moc_files
+  include/fuse_viz/mapped_covariance_property.h
+  include/fuse_viz/pose_2d_stamped_property.h
+  include/fuse_viz/relative_pose_2d_stamped_constraint_property.h
   include/fuse_viz/serialized_graph_display.h
 )
 
 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
+  src/relative_pose_2d_stamped_constraint_visual.cpp
   src/serialized_graph_display.cpp
   ${moc_files}
 )
@@ -49,10 +67,13 @@ add_dependencies(${PROJECT_NAME}
 target_include_directories(${PROJECT_NAME}
   PUBLIC
     include
+    ${Boost_INCLUDE_DIRS}
     ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
 )
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
   ${QT_LIBRARIES}
 )
 

fuse_viz/include/fuse_viz/conversions.h

@@ -0,0 +1,156 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Clearpath Robotics
+ *  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 copyright holder 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 HOLDER 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 FUSE_VIZ_CONVERSIONS_H
+#define FUSE_VIZ_CONVERSIONS_H
+
+#include <fuse_core/graph.h>
+#include <fuse_core/uuid.h>
+#include <fuse_variables/orientation_2d_stamped.h>
+#include <fuse_variables/position_2d_stamped.h>
+
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/LinearMath/Vector3.h>
+
+#include <OgreColourValue.h>
+#include <OgreQuaternion.h>
+#include <OgreVector3.h>
+
+#include <boost/array.hpp>
+
+#include <Eigen/Dense>
+
+#include <stdexcept>
+
+namespace tf2
+{
+
+/**
+ * @brief Convert a 3x3 covariance matrix into a 6x6 covariance message.
+ * @param[in]  covariance 3x3 covariance matrix.
+ * @param[out] msg        6x6 covariance message, which is stored as a plain array with 36 elements.
+ */
+template <typename Derived>
+inline void toMsg(const Eigen::MatrixBase<Derived>& covariance, boost::array<double, 36>& msg)
+{
+  using Scalar = typename Derived::Scalar;
+  using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
+
+  Eigen::Map<Matrix6> C(msg.data());
+  C.template topLeftCorner<2, 2>() = covariance.template topLeftCorner<2, 2>();
+  C(5, 5) = covariance(2, 2);
+
+  C(0, 5) = covariance(0, 2);
+  C(1, 5) = covariance(1, 2);
+  C(5, 0) = covariance(2, 0);
+  C(5, 1) = covariance(2, 1);
+}
+
+}  // namespace tf2
+
+namespace
+{
+
+inline tf2::Vector3 toTF(const fuse_variables::Position2DStamped& position)
+{
+  return { position.x(), position.y(), 0 };
+}
+
+inline tf2::Quaternion toTF(const fuse_variables::Orientation2DStamped& orientation)
+{
+  return { tf2::Vector3{ 0, 0, 1 }, orientation.yaw() };
+}
+
+inline tf2::Transform toTF(const fuse_variables::Position2DStamped& position,
+                           const fuse_variables::Orientation2DStamped& orientation)
+{
+  return tf2::Transform(toTF(orientation), toTF(position));
+}
+
+inline Ogre::Vector3 toOgre(const tf2::Vector3& position)
+{
+  return { static_cast<float>(position.x()), static_cast<float>(position.y()), static_cast<float>(position.z()) };
+}
+
+inline Ogre::Quaternion toOgre(const tf2::Quaternion& orientation)
+{
+  return { static_cast<float>(orientation.w()), static_cast<float>(orientation.x()),  // NOLINT(whitespace/braces)
+           static_cast<float>(orientation.y()), static_cast<float>(orientation.z()) };
+}
+
+inline Ogre::Vector3 toOgre(const fuse_variables::Position2DStamped& position)
+{
+  return { static_cast<float>(position.x()), static_cast<float>(position.y()), 0 };
+}
+
+inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped& orientation)
+{
+  return toOgre(toTF(orientation));
+}
+
+}  // namespace
+
+namespace
+{
+
+inline tf2::Transform getPose(const fuse_variables::Position2DStamped& position,
+                              const fuse_variables::Orientation2DStamped& orientation)
+{
+  return tf2::Transform(toTF(orientation), toTF(position));
+}
+
+inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UUID& position_uuid,
+                              const fuse_core::UUID& orientation_uuid)
+{
+  const auto position = dynamic_cast<const fuse_variables::Position2DStamped*>(&graph.getVariable(position_uuid));
+  if (!position)
+  {
+    throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) +
+                             " from graph as fuse_variables::Position2DStamped.");
+  }
+
+  const auto orientation =
+      dynamic_cast<const fuse_variables::Orientation2DStamped*>(&graph.getVariable(orientation_uuid));
+  if (!orientation)
+  {
+    throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) +
+                             " from graph as fuse_variables::Orientation2DStamped.");
+  }
+
+  return getPose(*position, *orientation);
+}
+
+}  // namespace
+#endif  // FUSE_VIZ_CONVERSIONS_H

fuse_viz/include/fuse_viz/mapped_covariance_property.h

@@ -0,0 +1,129 @@
+/*
+ * 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 FUSE_VIZ_MAPPED_COVARIANCE_PROPERTY_H
+#define FUSE_VIZ_MAPPED_COVARIANCE_PROPERTY_H
+
+#include <QColor>
+
+#include <OgreColourValue.h>
+
+#include <rviz/properties/bool_property.h>
+
+#include <string>
+#include <unordered_map>
+
+namespace Ogre
+{
+
+class SceneManager;
+class SceneNode;
+
+}  // namespace Ogre
+
+namespace rviz
+{
+
+class Property;
+class ColorProperty;
+class FloatProperty;
+class EnumProperty;
+class MappedCovarianceVisual;
+
+/**
+ * @brief Property specialized to provide getter for booleans.
+ *
+ * This is mostly a copy of CovarianceProperty from rviz/default_plugin/covariance_property.h that instead of using an
+ * std::deque to store the visuals, it uses an std::unordered_map, so the visuals can be indexed by their UUID.
+ */
+class MappedCovarianceProperty : public rviz::BoolProperty
+{
+  Q_OBJECT
+public:
+  typedef boost::shared_ptr<MappedCovarianceVisual> MappedCovarianceVisualPtr;
+
+  enum Frame
+  {
+    Local,
+    Fixed,
+  };
+
+  enum ColorStyle
+  {
+    Unique,
+    RGB,
+  };
+
+  MappedCovarianceProperty(const QString& name = "Covariance", bool default_value = false,
+                           const QString& description = QString(), rviz::Property* parent = 0,
+                           const char* changed_slot = 0, QObject* receiver = 0);
+
+  virtual ~MappedCovarianceProperty();
+
+  bool getPositionBool();
+  bool getOrientationBool();
+
+  // Methods to manage the unordered map of Covariance Visuals
+  MappedCovarianceVisualPtr createAndInsertVisual(const std::string& key, Ogre::SceneManager* scene_manager,
+                                                  Ogre::SceneNode* parent_node);
+  void eraseVisual(const std::string& key);
+  void clearVisual();
+  size_t sizeVisual();
+
+public Q_SLOTS:
+  void updateVisibility();
+
+private Q_SLOTS:
+  void updateColorAndAlphaAndScaleAndOffset();
+  void updateOrientationFrame();
+  void updateColorStyleChoice();
+
+private:
+  void updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr& visual);
+  void updateOrientationFrame(const MappedCovarianceVisualPtr& visual);
+  void updateVisibility(const MappedCovarianceVisualPtr& visual);
+
+  std::unordered_map<std::string, MappedCovarianceVisualPtr> covariances_;
+
+  rviz::BoolProperty* position_property_;
+  rviz::ColorProperty* position_color_property_;
+  rviz::FloatProperty* position_alpha_property_;
+  rviz::FloatProperty* position_scale_property_;
+  rviz::BoolProperty* orientation_property_;
+  rviz::EnumProperty* orientation_frame_property_;
+  rviz::EnumProperty* orientation_colorstyle_property_;
+  rviz::ColorProperty* orientation_color_property_;
+  rviz::FloatProperty* orientation_alpha_property_;
+  rviz::FloatProperty* orientation_offset_property_;
+  rviz::FloatProperty* orientation_scale_property_;
+};
+
+}  // end namespace rviz
+
+#endif  // FUSE_VIZ_MAPPED_COVARIANCE_PROPERTY_H