Feature/mesh point plugin

hydra_ros/CMakeLists.txt

@@ -115,6 +115,7 @@ install(
 install(TARGETS hydra_ros_node RUNTIME DESTINATION lib/${PROJECT_NAME})
 install(
   PROGRAMS app/csv_to_tf
+           app/dsg_file_publisher
            app/dsg_republisher
            app/odom_to_tf
            app/noisy_tf_publisher

hydra_ros/app/dsg_file_publisher

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+"""Script that loads and publishes a dsg from a file."""
+
+import argparse
+import pathlib
+
+import rclpy
+import spark_dsg as dsg
+import std_msgs.msg
+from rclpy.node import Node
+
+import hydra_ros
+
+
+class DsgFilePublisher(Node):
+    """Node that loads and publishes a DSG from file."""
+
+    def __init__(
+        self,
+        filepath,
+        frame: str = "map",
+        publish_rate: float = 0.0,
+        publish_with_mesh: bool = True,
+    ):
+        """Load and send the DSG."""
+        super().__init__("dsg_file_publisher")
+        filepath = pathlib.Path(filepath).expanduser().absolute()
+        if not filepath.exists():
+            self.get_logger().fatal(f"File '{filepath}' does not exist!")
+            raise ValueError(f"invalid file {filepath}")
+
+        self._frame = frame
+        self.get_logger().info(f"Loading '{filepath}'...")
+        self._graph = dsg.DynamicSceneGraph.load(filepath)
+        self.get_logger().info(f"Loaded '{filepath}'!")
+        self._pub = hydra_ros.DsgPublisher(self, "dsg")
+        if publish_rate > 0.0:
+            self._timer = self._create_timer(1.0 / publish_rate, self._send_graph)
+        else:
+            self._send_graph()
+
+    def _send_graph(self):
+        header = std_msgs.msg.Header()
+        header.stamp = self.get_clock().now().to_msg()
+        header.frame_id = self._frame
+        self._pub.publish_with_header(self._graph, header)
+
+
+def main(args=None):
+    """Start and run node."""
+    parser = argparse.ArgumentParser("Node that publishes a 3SDG from a file")
+    parser.add_argument("filepath")
+    parser.add_argument("--publish_rate", default=0.0, help="rate to publish")
+    parser.add_argument("--frame", "-f", default="map", help="frame ID for publishing")
+    args, rest = parser.parse_known_args()
+    rclpy.init(args=rest)
+
+    try:
+        node = DsgFilePublisher(
+            args.filepath, frame=args.frame, publish_rate=args.publish_rate
+        )
+        rclpy.spin(node)
+        node.destroy_node()
+    finally:
+        rclpy.utilities.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

hydra_visualizer/CMakeLists.txt

@@ -29,7 +29,6 @@ find_package(visualization_msgs REQUIRED)
 
 add_library(
   ${PROJECT_NAME}
-  src/plugins/traversability_plugin.cpp
   src/adapters/edge_color.cpp
   src/adapters/mesh_color.cpp
   src/adapters/node_color.cpp
@@ -46,8 +45,10 @@ add_library(
   src/plugins/footprint_plugin.cpp
   src/plugins/khronos_object_plugin.cpp
   src/plugins/mesh_plugin.cpp
+  src/plugins/mesh_point_plugin.cpp
   src/plugins/places_freespace_plugin.cpp
   src/plugins/pose_plugin.cpp
+  src/plugins/traversability_plugin.cpp
   src/scene_graph_renderer.cpp
   src/utils/ear_clipping.cpp
   src/utils/layer_key_selector.cpp

hydra_visualizer/config/visualizer_config.yaml

@@ -6,7 +6,7 @@ renderer:
     2:
       z_offset_scale: 2.0
       visualize: true
-      nodes: {scale: 0.40, color: {type: LabelColorAdapter}, alpha: 0.8, use_sphere: false}
+      nodes: {draw: true, scale: 0.40, color: {type: LabelColorAdapter}, alpha: 0.8, use_sphere: false}
       text: {draw: true, collapse: true, adapter: {type: LabelTextAdapter}, height: 0.5, scale: 0.45}
       bounding_boxes: {draw: true, collapse: true, scale: 0.05, edge_scale: 0.05, alpha: 0.9, edge_break_ratio: 0.5}
     3:
@@ -43,3 +43,4 @@ renderer:
     - {from: 3*, to: 2p*, draw: false}
     - {from: 3*, to: 2, use_child_color: true, scale: 0.08, alpha: 0.5}
     - {from: 4, to: 3*, scale: 0.08, alpha: 0.4}
+  layer_plugins: []

hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h

@@ -57,6 +57,8 @@ enum class NamedColors {
 
 spark_dsg::Color colorFromName(NamedColors color);
 
+void fillColorMsg(const spark_dsg::Color& color, std_msgs::msg::ColorRGBA& msg);
+
 std_msgs::msg::ColorRGBA makeColorMsg(const spark_dsg::Color& color,
                                       std::optional<double> alpha = std::nullopt);
 

hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h

@@ -0,0 +1,66 @@
+/* -----------------------------------------------------------------------------
+ * Copyright 2022 Massachusetts Institute of Technology.
+ * All Rights Reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *
+ *  2. 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.
+ *
+ * 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.
+ *
+ * Research was sponsored by the United States Air Force Research Laboratory and
+ * the United States Air Force Artificial Intelligence Accelerator and was
+ * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
+ * and conclusions contained in this document are those of the authors and should
+ * not be interpreted as representing the official policies, either expressed or
+ * implied, of the United States Air Force or the U.S. Government. The U.S.
+ * Government is authorized to reproduce and distribute reprints for Government
+ * purposes notwithstanding any copyright notation herein.
+ * -------------------------------------------------------------------------- */
+#pragma once
+#include <spark_dsg/dynamic_scene_graph.h>
+
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include "hydra_visualizer/layer_info.h"
+#include "hydra_visualizer/utils/marker_tracker.h"
+
+namespace hydra {
+
+struct LayerPlugin {
+ public:
+  using Ptr = std::unique_ptr<LayerPlugin>;
+
+  virtual ~LayerPlugin() = default;
+
+  virtual void draw(const std_msgs::msg::Header& header,
+                    const visualizer::LayerInfo& info,
+                    const spark_dsg::SceneGraphLayer& layer,
+                    const spark_dsg::Mesh* mesh,
+                    visualization_msgs::msg::MarkerArray& msg,
+                    MarkerTracker& tracker) = 0;
+
+  virtual bool hasChange() const { return has_change_; }
+
+  virtual void clearChangeFlag() { has_change_ = false; }
+
+ protected:
+  bool has_change_ = false;
+};
+
+}  // namespace hydra

hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h

@@ -0,0 +1,77 @@
+/* -----------------------------------------------------------------------------
+ * Copyright 2022 Massachusetts Institute of Technology.
+ * All Rights Reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *
+ *  2. 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.
+ *
+ * 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.
+ *
+ * Research was sponsored by the United States Air Force Research Laboratory and
+ * the United States Air Force Artificial Intelligence Accelerator and was
+ * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
+ * and conclusions contained in this document are those of the authors and should
+ * not be interpreted as representing the official policies, either expressed or
+ * implied, of the United States Air Force or the U.S. Government. The U.S.
+ * Government is authorized to reproduce and distribute reprints for Government
+ * purposes notwithstanding any copyright notation herein.
+ * -------------------------------------------------------------------------- */
+#pragma once
+
+#include <config_utilities/dynamic_config.h>
+
+#include "hydra_visualizer/plugins/layer_plugin.h"
+
+namespace hydra {
+
+/**
+ * @brief Plugin to draw mesh points that comprise a node
+ *
+ * Works for 2D places or objects
+ */
+class MeshPointPlugin : public LayerPlugin {
+ public:
+  struct Config {
+    //! Draw size for mesh points
+    double point_size = 0.02;
+    //! Use spheres for mesh points instead of cubes
+    bool use_spheres = false;
+    //! Use the node color instead of the mesh color
+    bool use_node_color = true;
+    //! Alpha for mesh points
+    double alpha = 1.0;
+  };
+
+  MeshPointPlugin(const Config& config, const std::string& ns);
+
+  void draw(const std_msgs::msg::Header& header,
+            const visualizer::LayerInfo& info,
+            const spark_dsg::SceneGraphLayer& layer,
+            const spark_dsg::Mesh* mesh,
+            visualization_msgs::msg::MarkerArray& msg,
+            MarkerTracker& tracker) override;
+
+ private:
+  std::string ns_;
+  config::DynamicConfig<Config> config_;
+};
+
+void declare_config(MeshPointPlugin::Config& config);
+
+}  // namespace hydra

hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h

@@ -40,6 +40,7 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include "hydra_visualizer/layer_info.h"
+#include "hydra_visualizer/plugins/layer_plugin.h"
 #include "hydra_visualizer/utils/layer_key_selector.h"
 #include "hydra_visualizer/utils/marker_tracker.h"
 
@@ -68,6 +69,11 @@ class SceneGraphRenderer {
   using LayerConfigWrapper = config::DynamicConfig<visualizer::LayerConfig>;
   using EdgeConfigWrapper = config::DynamicConfig<InterlayerEdgeConfig>;
 
+  struct LayerPluginsConfig {
+    spark_dsg::LayerKey layer;
+    std::vector<config::VirtualConfig<LayerPlugin, true>> plugins;
+  };
+
   struct Config {
     //! @brief Overall graph config
     GraphRenderConfig graph;
@@ -83,6 +89,8 @@ class SceneGraphRenderer {
     };
     //! @brief Configuration for interlayer edges
     std::vector<InterlayerEdges> interlayer_edges;
+    //! @brief Extra per-layer plugins
+    std::vector<LayerPluginsConfig> layer_plugins;
   };
 
   explicit SceneGraphRenderer(const Config& config, ianvs::NodeHandle nh);
@@ -124,6 +132,7 @@ class SceneGraphRenderer {
   ianvs::NodeHandle nh_;
   config::DynamicConfig<GraphRenderConfig> graph_config_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_;
+  std::map<spark_dsg::LayerKey, std::list<LayerPlugin::Ptr>> layer_plugins_;
 
   mutable MarkerTracker tracker_;
   mutable std::atomic<bool> has_change_;
@@ -132,6 +141,7 @@ class SceneGraphRenderer {
   mutable std::map<std::string, std::unique_ptr<EdgeConfigWrapper>> interlayer_edges_;
 };
 
+void declare_config(SceneGraphRenderer::LayerPluginsConfig& config);
 void declare_config(SceneGraphRenderer::Config& config);
 
 }  // namespace hydra

hydra_visualizer/include/hydra_visualizer/utils/layer_key_selector.h

@@ -70,4 +70,12 @@ struct SelectorConversion {
                                std::string& error);
 };
 
+struct LayerKeyConversion {
+  static std::string toIntermediate(const spark_dsg::LayerKey& value,
+                                    std::string& error);
+  static void fromIntermediate(const std::string& intermediate,
+                               spark_dsg::LayerKey& value,
+                               std::string& error);
+};
+
 }  // namespace hydra

hydra_visualizer/src/color/colormap_utilities.cpp

@@ -120,11 +120,15 @@ spark_dsg::Color colorFromName(NamedColors color) {
   }
 }
 
-std_msgs::msg::ColorRGBA makeColorMsg(const Color& color, std::optional<double> alpha) {
-  std_msgs::msg::ColorRGBA msg;
+void fillColorMsg(const Color& color, std_msgs::msg::ColorRGBA& msg) {
   msg.r = static_cast<double>(color.r) / 255.0;
   msg.g = static_cast<double>(color.g) / 255.0;
   msg.b = static_cast<double>(color.b) / 255.0;
+}
+
+std_msgs::msg::ColorRGBA makeColorMsg(const Color& color, std::optional<double> alpha) {
+  std_msgs::msg::ColorRGBA msg;
+  fillColorMsg(color, msg);
   msg.a = alpha.value_or(static_cast<double>(color.a) / 255.0);
   return msg;
 }

hydra_visualizer/src/plugins/mesh_plugin.cpp

@@ -38,7 +38,6 @@
 #include <config_utilities/validation.h>
 #include <glog/logging.h>
 
-#include "hydra_visualizer/color/colormap_utilities.h"
 #include "hydra_visualizer/drawing.h"
 
 namespace hydra {