diff --git a/fuse/package.xml b/fuse/package.xml index 559361cd3..3a74fd374 100644 --- a/fuse/package.xml +++ b/fuse/package.xml @@ -19,6 +19,7 @@ fuse_optimizers fuse_publishers fuse_variables + fuse_viz diff --git a/fuse_viz/CMakeLists.txt b/fuse_viz/CMakeLists.txt new file mode 100644 index 000000000..51c111519 --- /dev/null +++ b/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() diff --git a/fuse_viz/include/fuse_viz/serialized_graph_display.h b/fuse_viz/include/fuse_viz/serialized_graph_display.h new file mode 100644 index 000000000..10a8a1c3c --- /dev/null +++ b/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 +#include + +#include +#endif // Q_MOC_RUN + +#include + +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 +{ + 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 graph_shapes_; + + BoolProperty* draw_variables_axes_property_; + FloatProperty* scale_property_; + + fuse_core::GraphDeserializer graph_deserializer_; +}; + +} // namespace rviz + +#endif // FUSE_VIZ_SERIALIZED_GRAPH_DISPLAY_H diff --git a/fuse_viz/include/fuse_viz/serialized_graph_dot_writer.h b/fuse_viz/include/fuse_viz/serialized_graph_dot_writer.h new file mode 100644 index 000000000..a8e116c29 --- /dev/null +++ b/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 +#include +#include +#include + +#include + +#include + +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; + + // 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 diff --git a/fuse_viz/package.xml b/fuse_viz/package.xml new file mode 100644 index 000000000..eb0a89b6f --- /dev/null +++ b/fuse_viz/package.xml @@ -0,0 +1,21 @@ + + + fuse_viz + 0.4.0 + + The fuse_viz package provides visualization tools for fuse. + + + Enrique Fernandez + Enrique Fernandez + BSD + + catkin + fuse_msgs + fuse_variables + rviz + + + + + diff --git a/fuse_viz/rviz_plugins.xml b/fuse_viz/rviz_plugins.xml new file mode 100644 index 000000000..6b153c576 --- /dev/null +++ b/fuse_viz/rviz_plugins.xml @@ -0,0 +1,7 @@ + + + + An rviz display for fuse_msgs::SerializedGraph messages. + + + diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp new file mode 100644 index 000000000..193f4f4a5 --- /dev/null +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -0,0 +1,193 @@ +/* + * 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 Q_MOC_RUN +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#endif // Q_MOC_RUN + +#include + +#include +#include +#include + +namespace rviz +{ + +SerializedGraphDisplay::SerializedGraphDisplay() +{ + draw_variables_axes_property_ = new BoolProperty("Draw Variables as Axes", true, + "Whether to draw graph variables axes or not (spheres are always " + "drawn)", + this, SLOT(updateDrawVariablesAxesProperty())); + + scale_property_ = new FloatProperty("Scale", 0.1, "Scale of graph markers drawn", this, SLOT(updateScaleProperty())); +} + +SerializedGraphDisplay::~SerializedGraphDisplay() +{ + if (initialized()) + { + clear(); + root_node_->removeAndDestroyAllChildren(); + scene_manager_->destroySceneNode(root_node_->getName()); + } +} + +void SerializedGraphDisplay::reset() +{ + MFDClass::reset(); + clear(); +} + +void SerializedGraphDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + root_node_ = scene_node_->createChildSceneNode(); + + variables_axes_node_ = root_node_->createChildSceneNode(); + variables_spheres_node_ = root_node_->createChildSceneNode(); +} + +void SerializedGraphDisplay::onEnable() +{ + MFDClass::onEnable(); + + root_node_->setVisible(true); +} + +void SerializedGraphDisplay::onDisable() +{ + MFDClass::onDisable(); + + root_node_->setVisible(false); +} + +void SerializedGraphDisplay::updateDrawVariablesAxesProperty() +{ + variables_axes_node_->setVisible(draw_variables_axes_property_->getBool()); +} + +void SerializedGraphDisplay::updateScaleProperty() +{ + // TODO(efernandez) Redraw all variables so the scale is applied to the current graph. This is useful if no new + // message is received +} + +void SerializedGraphDisplay::clear() +{ + for (auto& graph_shape : graph_shapes_) + { + delete graph_shape; + } + graph_shapes_.clear(); +} + +void SerializedGraphDisplay::processMessage(const fuse_msgs::SerializedGraph::ConstPtr& msg) +{ + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) + { + ROS_DEBUG_STREAM("Error transforming from frame '" << msg->header.frame_id << "' to frame '" + << qPrintable(fixed_frame_) << "'"); + } + + root_node_->setPosition(position); + root_node_->setOrientation(orientation); + + clear(); + + const auto graph = graph_deserializer_.deserialize(msg); + + const Ogre::Vector3 scale(scale_property_->getFloat()); + + for (const auto& variable : graph->getVariables()) + { + const auto orientation = dynamic_cast(&variable); + if (!orientation) + { + continue; + } + + const auto& stamp = orientation->stamp(); + const auto position_uuid = fuse_variables::Position2DStamped(stamp, orientation->deviceId()).uuid(); + if (!graph->variableExists(position_uuid)) + { + continue; + } + + const auto position = dynamic_cast(&graph->getVariable(position_uuid)); + + const tf2::Quaternion tf_q(tf2::Vector3(0, 0, 1), orientation->yaw()); + + const Ogre::Vector3 p(position->x(), position->y(), 0); + const Ogre::Quaternion q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z()); + + // Add sphere: + Object* sphere = new rviz::Shape(rviz::Shape::Sphere, scene_manager_, variables_spheres_node_); + sphere->setPosition(p); + sphere->setScale(scale); + sphere->setColor(1.0, 0.0, 0.0, 1.0); + graph_shapes_.push_back(sphere); + + // Add axes: + Object* axes = new rviz::Axes(scene_manager_, variables_axes_node_, 10.0, 1.0); + axes->setPosition(p); + axes->setOrientation(q); + axes->setScale(scale); + graph_shapes_.push_back(axes); + } + + updateDrawVariablesAxesProperty(); +} + +} // namespace rviz + +#include +PLUGINLIB_EXPORT_CLASS(rviz::SerializedGraphDisplay, rviz::Display) diff --git a/fuse_viz/src/serialized_graph_dot_writer.cpp b/fuse_viz/src/serialized_graph_dot_writer.cpp new file mode 100644 index 000000000..579609371 --- /dev/null +++ b/fuse_viz/src/serialized_graph_dot_writer.cpp @@ -0,0 +1,164 @@ +/* + * 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. + */ + +#include + +#include + +#include + +#include +#include + +namespace fuse_viz +{ + +/** + * @brief Vertex writer. + * + * This tells BGL how to write the vertex properites into the Graphviz DOT format. + * This class implements a functor operator() that it's called for each vertex in the graph. + * + * @tparam UUIDMap Vertex property UUID mapping. + * @tparam TypeMap Vertex property type mapping. + * @tparam SubTypeMap Vertex property subtype mapping. + */ +template +class vertex_writer +{ +public: + /** + * @brief Vertex writer constructor. + * + * @param[in] uuid_map Vertex property UUID mapping. + * @param[in] type_map Vertex property type mapping. + * @param[in] subtype_map Vertex property subtype mapping. + */ + vertex_writer(UUIDMap uuid_map, TypeMap type_map, SubTypeMap subtype_map) + : uuid_map_(uuid_map), type_map_(type_map), subtype_map_(subtype_map) + { + } + + /** + * @brief Functor operator() the writes the vertex properties in DOT format. + * This writes the vertex properties as field-value pairs for each node/vertex into the DOT format. + * + * @param[in, out] os Output stream. + * @param[in] v Vertex. + * + * @tparam Vertex Vertex descriptor in the BGL graph. + */ + template + void operator()(std::ostream& os, const Vertex& v) const + { + os << "[" + << "shape=" << TYPE_SHAPES[type_map_[v]] << ", label=\"" << uuid_map_[v] << "\"" + << ", taillabel=\"" << subtype_map_[v] << "\"" + << "]"; + } + +private: + UUIDMap uuid_map_; //< Vertex property UUID mapping. + TypeMap type_map_; //< Vertex property type mapping. + SubTypeMap subtype_map_; //< Vertex property subtype mapping. + + static const char* const TYPE_SHAPES[]; //< Graphviz DOT shape strings for each vertex property type supported. +}; + +// The shapes for each type supported: +// * VARIABLE -> circle +// * CONSTRAINT -> diamond +template +const char* const vertex_writer::TYPE_SHAPES[] = { "circle", "diamond", "none" }; + +/** + * @brief A helper function to make/create a vertex writer. + * + * @param[in] uuid_map Vertex property UUID mapping. + * @param[in] type_map Vertex property type mapping. + * @param[in] subtype_map Vertex property subtype mapping. + * + * @tparam UUIDMap Vertex property UUID mapping. + * @tparam TypeMap Vertex property type mapping. + * @tparam SubTypeMap Vertex property subtype mapping. + */ +template +inline vertex_writer make_vertex_writer(UUIDMap uuid_map, TypeMap type_map, + SubTypeMap subtype_map) +{ + return vertex_writer(uuid_map, type_map, subtype_map); +} + +SerializedGraphDOTWriter::Graph SerializedGraphDOTWriter::toBoost(const fuse_msgs::SerializedGraphConstPtr& msg) +{ + static fuse_core::GraphDeserializer graph_deserializer; + + using VertexMap = std::map; + + const auto graph = graph_deserializer.deserialize(msg); + + Graph boost_graph; + + VertexMap variable_vertex_by_uuid; + for (const auto& variable : graph->getVariables()) + { + variable_vertex_by_uuid[fuse_core::uuid::to_string(variable.uuid())] = + boost::add_vertex(VertexProperty(variable), boost_graph); + } + + for (const auto& constraint : graph->getConstraints()) + { + const auto constraint_vertex = boost::add_vertex(VertexProperty(constraint), boost_graph); + + for (const auto& variable_uuid : constraint.variables()) + { + boost::add_edge(constraint_vertex, variable_vertex_by_uuid[fuse_core::uuid::to_string(variable_uuid)], + boost_graph); + } + } + + return boost_graph; +} + +std::ostream& SerializedGraphDOTWriter::write(std::ostream& os, const fuse_msgs::SerializedGraphConstPtr& msg) +{ + const auto boost_graph = toBoost(msg); + boost::write_graphviz(os, boost_graph, + make_vertex_writer(boost::get(&VertexProperty::uuid, boost_graph), + boost::get(&VertexProperty::type, boost_graph), + boost::get(&VertexProperty::subtype, boost_graph))); + return os; +} + +} // namespace fuse_viz diff --git a/fuse_viz/src/serialized_graph_dot_writer_node.cpp b/fuse_viz/src/serialized_graph_dot_writer_node.cpp new file mode 100644 index 000000000..89d2f2031 --- /dev/null +++ b/fuse_viz/src/serialized_graph_dot_writer_node.cpp @@ -0,0 +1,98 @@ +/* + * 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. + */ + +#include +#include + +#include +#include + +namespace fuse_viz +{ + +/** + * @brief A fuse_msgs::SerializedGraph to DOT format writer node. + * + * This subscribes to a fuse_msgs::SerializedGraph topic and saves each message into a file using Graphviz DOT format. + */ +class SerializedGraphDOTWriterNode +{ +public: + /** + * @brief Constructor. + * This reads ROS parameters and subscribes to the fuse_msgs::SerializedGraph topic. + */ + SerializedGraphDOTWriterNode(); + + /** + * @brief A callback to process fuse_msgs::SerializedGraph messages. + * + * @param[in] msg A fuse_msgs::SerializedGraph message. + */ + void graphCallback(const fuse_msgs::SerializedGraphConstPtr& msg); + +private: + ros::NodeHandle nh_; //< ROS node handle + ros::Subscriber subscriber_; //< ROS subscriber to the fuse_msgs::SerializedGraph topic + + std::string filename_prefix_{ "" }; //< Filename prefix to save the graph in Graphviz DOT format + int queue_size_{ 100 }; //< Queue size for the ROS subscriber +}; + +SerializedGraphDOTWriterNode::SerializedGraphDOTWriterNode() +{ + ros::NodeHandle private_node_handle("~"); + + private_node_handle.param("filename_prefix", filename_prefix_, filename_prefix_); + private_node_handle.param("queue_size", queue_size_, queue_size_); + + subscriber_ = nh_.subscribe("graph", queue_size_, &SerializedGraphDOTWriterNode::graphCallback, this); +} + +void SerializedGraphDOTWriterNode::graphCallback(const fuse_msgs::SerializedGraphConstPtr& msg) +{ + std::ofstream ofs(filename_prefix_ + std::to_string(msg->header.seq) + ".dot"); + SerializedGraphDOTWriter::write(ofs, msg); +} + +} // namespace fuse_viz + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "serialized_graph_dot_writer_node"); + fuse_viz::SerializedGraphDOTWriterNode node; + ros::spin(); + + return EXIT_SUCCESS; +}