Add SerializedGraph to GraphViz DOT writer

fuse/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>fuse_optimizers</exec_depend>
   <exec_depend>fuse_publishers</exec_depend>
   <exec_depend>fuse_variables</exec_depend>
+  <exec_depend>fuse_viz</exec_depend>
 
   <export>
     <metapackage />

fuse_viz/CMakeLists.txt

@@ -0,0 +1,109 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(fuse_viz)
+
+set(build_depends
+  fuse_msgs
+  fuse_variables
+  rviz
+)
+
+find_package(catkin REQUIRED COMPONENTS
+  ${build_depends}
+)
+
+find_package(Boost REQUIRED COMPONENTS graph)
+
+find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
+set(QT_LIBRARIES Qt5::Widgets)
+
+add_definitions(-DQT_NO_KEYWORDS)
+
+###########
+## Build ##
+###########
+
+add_compile_options(-std=c++14 -Wall -Werror)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    ${build_depends}
+  DEPENDS
+    Boost
+    QT
+)
+
+qt5_wrap_cpp(moc_files
+  include/fuse_viz/serialized_graph_display.h
+)
+
+set(source_files
+  src/serialized_graph_display.cpp
+  ${moc_files}
+)
+
+add_library(${PROJECT_NAME}
+  ${source_files}
+)
+add_dependencies(${PROJECT_NAME}
+  ${catkin_EXPORTED_TARGETS}
+)
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+    include
+    ${catkin_INCLUDE_DIRS}
+)
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${QT_LIBRARIES}
+)
+
+add_executable(serialized_graph_dot_writer_node
+  src/serialized_graph_dot_writer.cpp
+  src/serialized_graph_dot_writer_node.cpp
+)
+add_dependencies(serialized_graph_dot_writer_node
+  ${catkin_EXPORTED_TARGETS}
+)
+target_include_directories(serialized_graph_dot_writer_node
+  PUBLIC
+    include
+    ${catkin_INCLUDE_DIRS}
+)
+target_link_libraries(serialized_graph_dot_writer_node
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(TARGETS ${PROJECT_NAME} serialized_graph_dot_writer_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(
+  FILES rviz_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(roslint REQUIRED)
+
+  # Lint tests
+  set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references")
+  roslint_cpp()
+  roslint_add_test()
+endif()

fuse_viz/include/fuse_viz/serialized_graph_display.h

@@ -0,0 +1,104 @@
+/*
+ * 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_SERIALIZED_GRAPH_DISPLAY_H
+#define FUSE_VIZ_SERIALIZED_GRAPH_DISPLAY_H
+
+#ifndef Q_MOC_RUN
+#include <fuse_core/graph_deserializer.h>
+#include <fuse_msgs/SerializedGraph.h>
+
+#include <rviz/message_filter_display.h>
+#endif  // Q_MOC_RUN
+
+#include <vector>
+
+namespace Ogre
+{
+class SceneNode;
+}
+
+namespace rviz
+{
+
+class Object;
+
+class BoolProperty;
+class FloatProperty;
+
+/**
+ * @brief An rviz dispaly for fuse_msgs::SerializedGraph messages.
+ */
+class SerializedGraphDisplay : public MessageFilterDisplay<fuse_msgs::SerializedGraph>
+{
+  Q_OBJECT
+public:
+  SerializedGraphDisplay();
+
+  ~SerializedGraphDisplay() override;
+
+  void reset() override;
+
+protected:
+  void onInitialize() override;
+
+  void onEnable() override;
+
+  void onDisable() override;
+
+private Q_SLOTS:
+  void updateDrawVariablesAxesProperty();
+
+  void updateScaleProperty();
+
+private:
+  void clear();
+
+  void processMessage(const fuse_msgs::SerializedGraph::ConstPtr& msg) override;
+
+  Ogre::SceneNode* root_node_;
+  Ogre::SceneNode* variables_axes_node_;
+  Ogre::SceneNode* variables_spheres_node_;
+
+  std::vector<Object*> graph_shapes_;
+
+  BoolProperty* draw_variables_axes_property_;
+  FloatProperty* scale_property_;
+
+  fuse_core::GraphDeserializer graph_deserializer_;
+};
+
+}  // namespace rviz
+
+#endif  // FUSE_VIZ_SERIALIZED_GRAPH_DISPLAY_H

fuse_viz/include/fuse_viz/serialized_graph_dot_writer.h

@@ -0,0 +1,145 @@
+/*
+ * 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_SERIALIZED_GRAPH_DOT_WRITER_H
+#define FUSE_VIZ_SERIALIZED_GRAPH_DOT_WRITER_H
+
+#include <fuse_core/constraint.h>
+#include <fuse_core/uuid.h>
+#include <fuse_core/variable.h>
+#include <fuse_msgs/SerializedGraph.h>
+
+#include <boost/graph/adjacency_list.hpp>
+
+#include <string>
+
+namespace fuse_viz
+{
+
+/**
+ * @brief A fuse_msgs::SerializedGraph to Graphviz DOT format writer.
+ *
+ * This converts a fuse_msgs::SerializedGraph message into a BGL (Boost Graph Library) graph that can be easily
+ * converted/serialized into a Graphviz DOT format. The DOT format is serialized into an std::ostream.
+ */
+class SerializedGraphDOTWriter
+{
+public:
+  /**
+   * @brief A vertex property bundle to store inside the BGL graph.
+   *
+   * This allows to store the fields from the variables and constraints in a fuse_msgs::SerializedGraph message. This
+   * information is stored as part of the BGL graph as a property bundle, so later we can easily access that
+   * information.
+   */
+  class VertexProperty
+  {
+  public:
+    /**
+     * @brief Vertex property type.
+     *
+     * A vertex property can either be a VARIABLE or a CONSTRAINT. The default constructor uses the UNKOWN type though.
+     */
+    enum Type
+    {
+      VARIABLE,    //< Variable type
+      CONSTRAINT,  //< Constraint type
+      UNKNOWN      //< Unknown type
+    };
+
+    /**
+     * @brief Vertex property constructor.
+     */
+    VertexProperty(const std::string& uuid = fuse_core::uuid::to_string(fuse_core::uuid::NIL),
+                   const Type type = UNKNOWN, const std::string& subtype = "NO_SUBTYPE")
+      : uuid(uuid), type(type), subtype(subtype)
+    {
+    }
+
+    /**
+     * @brief Vertex property constructor from a fuse_core::Variable.
+     * The type is set to VARIABLE. The variable type is used to set the subtype.
+     *
+     * @param[in] variable A fuse_core::Variable.
+     */
+    explicit VertexProperty(const fuse_core::Variable& variable)
+      : uuid(fuse_core::uuid::to_string(variable.uuid())), type(VARIABLE), subtype(variable.type())
+    {
+    }
+
+    /**
+     * @brief Vertex property constructor from a fuse_core::Constraint.
+     * The type is set to CONSTRAINT. The constraint type is used to set the subtype.
+     *
+     * @param[in] constraint A fuse_core::Constraint.
+     */
+    explicit VertexProperty(const fuse_core::Constraint& constraint)
+      : uuid(fuse_core::uuid::to_string(constraint.uuid())), type(CONSTRAINT), subtype(constraint.type())
+    {
+    }
+
+    std::string uuid;     //< Variable/Constraint UUID
+    Type type;            //< VARIABLE or CONTRAINT type
+    std::string subtype;  //< Variable/Constraint type
+  };
+
+  // Graph typedef
+  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, VertexProperty>;
+
+  // Disable constructor and destructor, since this class only provides static methods
+  SerializedGraphDOTWriter() = delete;
+  ~SerializedGraphDOTWriter() = delete;
+
+  /**
+   * @brief Converts a fuse_msgs::SerializedGraph message into a BGL graph.
+   *
+   * @param[in] msg A fuse_msgs::SerializedGraph message.
+   * @return A BGL graph.
+   */
+  static Graph toBoost(const fuse_msgs::SerializedGraphConstPtr& msg);
+
+  /**
+   * @brief Write a fuse_msgs::SerializedGraph message into an std::ostream.
+   * The message is first converted into a BGL graph and then serialized as a Graphviz DOT format.
+   *
+   * @param[in, out] os  Output stream.
+   * @param[in]      msg A fuse_msgs::SerializedGraph message.
+   * @return Output stream with the fuse_msgs::SerializedGraph serialized into it.
+   */
+  static std::ostream& write(std::ostream& os, const fuse_msgs::SerializedGraphConstPtr& msg);
+};
+
+}  // namespace fuse_viz
+
+#endif  // FUSE_VIZ_SERIALIZED_GRAPH_DOT_WRITER_H

fuse_viz/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>fuse_viz</name>
+  <version>0.4.0</version>
+  <description>
+    The fuse_viz package provides visualization tools for fuse.
+  </description>
+
+  <maintainer email="[email protected]">Enrique Fernandez</maintainer>
+  <author email="[email protected]">Enrique Fernandez</author>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>fuse_msgs</depend>
+  <depend>fuse_variables</depend>
+  <depend>rviz</depend>
+
+  <export>
+    <rviz plugin="${prefix}/rviz_plugins.xml"/>
+  </export>
+</package>

fuse_viz/rviz_plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libfuse_viz">
+  <class name="fuse_viz/SerializedGraph" type="rviz::SerializedGraphDisplay" base_class_type="rviz::Display">
+    <description>
+    An rviz display for fuse_msgs::SerializedGraph messages.
+    </description>
+  </class>
+</library>