diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index 32a0bfd..0c5b70d 100644 --- a/hydra_ros/CMakeLists.txt +++ b/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 diff --git a/hydra_ros/app/dsg_file_publisher b/hydra_ros/app/dsg_file_publisher new file mode 100755 index 0000000..59b2b0a --- /dev/null +++ b/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() diff --git a/hydra_visualizer/CMakeLists.txt b/hydra_visualizer/CMakeLists.txt index c200f82..ea51377 100644 --- a/hydra_visualizer/CMakeLists.txt +++ b/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 diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index eb98b84..376fa3e 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/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: [] diff --git a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h index 4fc3ff7..50c9026 100644 --- a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h +++ b/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 alpha = std::nullopt); diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h new file mode 100644 index 0000000..1898fc0 --- /dev/null +++ b/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 + +#include + +#include "hydra_visualizer/layer_info.h" +#include "hydra_visualizer/utils/marker_tracker.h" + +namespace hydra { + +struct LayerPlugin { + public: + using Ptr = std::unique_ptr; + + 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 diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h new file mode 100644 index 0000000..ad98ea1 --- /dev/null +++ b/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 + +#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_; +}; + +void declare_config(MeshPointPlugin::Config& config); + +} // namespace hydra diff --git a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h index c257ca6..f56424e 100644 --- a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h +++ b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h @@ -40,6 +40,7 @@ #include #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; using EdgeConfigWrapper = config::DynamicConfig; + struct LayerPluginsConfig { + spark_dsg::LayerKey layer; + std::vector> plugins; + }; + struct Config { //! @brief Overall graph config GraphRenderConfig graph; @@ -83,6 +89,8 @@ class SceneGraphRenderer { }; //! @brief Configuration for interlayer edges std::vector interlayer_edges; + //! @brief Extra per-layer plugins + std::vector layer_plugins; }; explicit SceneGraphRenderer(const Config& config, ianvs::NodeHandle nh); @@ -124,6 +132,7 @@ class SceneGraphRenderer { ianvs::NodeHandle nh_; config::DynamicConfig graph_config_; rclcpp::Publisher::SharedPtr pub_; + std::map> layer_plugins_; mutable MarkerTracker tracker_; mutable std::atomic has_change_; @@ -132,6 +141,7 @@ class SceneGraphRenderer { mutable std::map> interlayer_edges_; }; +void declare_config(SceneGraphRenderer::LayerPluginsConfig& config); void declare_config(SceneGraphRenderer::Config& config); } // namespace hydra diff --git a/hydra_visualizer/include/hydra_visualizer/utils/layer_key_selector.h b/hydra_visualizer/include/hydra_visualizer/utils/layer_key_selector.h index 842f957..5cac9e5 100644 --- a/hydra_visualizer/include/hydra_visualizer/utils/layer_key_selector.h +++ b/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 diff --git a/hydra_visualizer/src/color/colormap_utilities.cpp b/hydra_visualizer/src/color/colormap_utilities.cpp index 8ede867..117851d 100644 --- a/hydra_visualizer/src/color/colormap_utilities.cpp +++ b/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 alpha) { - std_msgs::msg::ColorRGBA msg; +void fillColorMsg(const Color& color, std_msgs::msg::ColorRGBA& msg) { msg.r = static_cast(color.r) / 255.0; msg.g = static_cast(color.g) / 255.0; msg.b = static_cast(color.b) / 255.0; +} + +std_msgs::msg::ColorRGBA makeColorMsg(const Color& color, std::optional alpha) { + std_msgs::msg::ColorRGBA msg; + fillColorMsg(color, msg); msg.a = alpha.value_or(static_cast(color.a) / 255.0); return msg; } diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index 2465204..df9f584 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_plugin.cpp @@ -38,7 +38,6 @@ #include #include -#include "hydra_visualizer/color/colormap_utilities.h" #include "hydra_visualizer/drawing.h" namespace hydra { diff --git a/hydra_visualizer/src/plugins/mesh_point_plugin.cpp b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp new file mode 100644 index 0000000..974ad2c --- /dev/null +++ b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp @@ -0,0 +1,134 @@ +#include "hydra_visualizer/plugins/mesh_point_plugin.h" + +#include + +#include "hydra_visualizer/color/colormap_utilities.h" + +namespace hydra { + +using spark_dsg::ObjectNodeAttributes; +using spark_dsg::Place2dNodeAttributes; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +namespace { + +static const auto registration = + config::RegistrationWithConfig("MeshPointPlugin"); + +void fillMarker(const MeshPointPlugin::Config& config, + const visualizer::LayerInfo& info, + const Place2dNodeAttributes& attrs, + const spark_dsg::Mesh& mesh, + const spark_dsg::Color& node_color, + Marker& marker) { + for (const auto idx : attrs.mesh_connections) { + const auto& pos = mesh.pos(idx); + auto& point = marker.points.emplace_back(); + point.x = pos.x(); + point.y = pos.y(); + point.z = pos.z() + info.z_offset; + + auto& color = marker.colors.emplace_back(); + color.a = config.alpha; + if (!config.use_node_color && mesh.has_colors) { + visualizer::fillColorMsg(mesh.color(idx), color); + } else { + visualizer::fillColorMsg(node_color, color); + } + } +} + +void fillMarker(const MeshPointPlugin::Config& config, + const visualizer::LayerInfo& info, + const ObjectNodeAttributes& attrs, + const spark_dsg::Mesh& mesh, + const spark_dsg::Color& node_color, + Marker& marker) { + for (const auto idx : attrs.mesh_connections) { + const auto& pos = mesh.pos(idx); + auto& point = marker.points.emplace_back(); + point.x = pos.x(); + point.y = pos.y(); + point.z = pos.z() + info.z_offset; + + auto& color = marker.colors.emplace_back(); + color.a = config.alpha; + if (!config.use_node_color && mesh.has_colors) { + visualizer::fillColorMsg(mesh.color(idx), color); + } else { + visualizer::fillColorMsg(node_color, color); + } + } +} + +} // namespace + +void declare_config(MeshPointPlugin::Config& config) { + using namespace config; + name("MeshPointPlugin::Config"); + field(config.point_size, "point_size"); + field(config.use_spheres, "use_spheres"); + field(config.use_node_color, "use_node_color"); + field(config.alpha, "alpha"); + check(config.point_size, GT, 0.0, "point_size"); + checkInRange(config.alpha, 0.0, 1.0, "alpha"); +} + +MeshPointPlugin::MeshPointPlugin(const Config& config, const std::string& ns) + : ns_(ns), + config_(ns + "_mesh_point_plugin", config, [this]() { has_change_ = true; }) {} + +void MeshPointPlugin::draw(const std_msgs::msg::Header& header, + const visualizer::LayerInfo& info, + const spark_dsg::SceneGraphLayer& layer, + const spark_dsg::Mesh* mesh, + MarkerArray& msg, + MarkerTracker& tracker) { + if (!mesh) { + return; + } + + const auto config = config_.get(); + + Marker marker; + marker.header = header; + marker.type = config.use_spheres ? Marker::SPHERE_LIST : Marker::CUBE_LIST; + marker.action = Marker::ADD; + marker.id = 0; + marker.ns = ns_ + "_mesh_points"; + marker.scale.x = config.point_size; + marker.scale.y = config.point_size; + marker.scale.z = config.point_size; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + + marker.points.reserve(layer.numNodes()); + marker.colors.reserve(layer.numNodes()); + for (const auto& [node_id, node] : layer.nodes()) { + if (info.filter && !info.filter(*node)) { + continue; + } + + const auto color = info.node_color(*node); + auto obj_attrs = node->tryAttributes(); + if (obj_attrs) { + fillMarker(config, info, *obj_attrs, *mesh, color, marker); + continue; + } + + auto place_attrs = node->tryAttributes(); + if (place_attrs) { + fillMarker(config, info, *place_attrs, *mesh, color, marker); + continue; + } + } + + tracker.add(marker, msg); +} + +} // namespace hydra diff --git a/hydra_visualizer/src/scene_graph_renderer.cpp b/hydra_visualizer/src/scene_graph_renderer.cpp index 3fd6c86..f4456b7 100644 --- a/hydra_visualizer/src/scene_graph_renderer.cpp +++ b/hydra_visualizer/src/scene_graph_renderer.cpp @@ -136,12 +136,20 @@ void declare_config(GraphRenderConfig& config) { field(config.collapse_layers, "collapse_layers"); } +void declare_config(SceneGraphRenderer::LayerPluginsConfig& config) { + using namespace config; + name("SceneGraphRenderer::LayerPluginsConfig"); + field(config.layer, "layer"); + field(config.plugins, "plugins"); +} + void declare_config(SceneGraphRenderer::Config& config) { using namespace config; name("SceneGraphRenderer::Config"); field(config.graph, "graph"); field>(config.layers, "layers"); field(config.interlayer_edges, "interlayer_edges"); + field(config.layer_plugins, "layer_plugins"); } SceneGraphRenderer::SceneGraphRenderer(const Config& config, ianvs::NodeHandle nh) @@ -149,7 +157,15 @@ SceneGraphRenderer::SceneGraphRenderer(const Config& config, ianvs::NodeHandle n nh_(nh), graph_config_("scene_graph", config.graph, [this]() { has_change_ = true; }), pub_(nh.create_publisher("graph", rclcpp::QoS(1).transient_local())), - has_change_(false) {} + has_change_(false) { + for (const auto& plugin_config : config.layer_plugins) { + const auto key = plugin_config.layer; + auto& plugin_list = layer_plugins_[key]; + for (const auto& plugin : plugin_config.plugins) { + plugin_list.emplace_back(plugin.create(keyToLayerName(key))); + } + } +} void SceneGraphRenderer::reset(const std_msgs::msg::Header& header) { MarkerArray msg; @@ -182,7 +198,6 @@ void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, MarkerArray edges; drawInterlayerEdges(header, graph, edges); tracker_.add(edges, msg); - tracker_.clearPrevious(header, msg); if (!msg.markers.empty()) { pub_->publish(msg); @@ -297,7 +312,7 @@ void SceneGraphRenderer::drawInterlayerEdges(const std_msgs::msg::Header& header void SceneGraphRenderer::drawLayer(const std_msgs::msg::Header& header, const LayerInfo& info, const SceneGraphLayer& layer, - const Mesh* /* mesh */, + const Mesh* mesh, MarkerArray& msg) const { if (!info.config.visualize) { return; @@ -311,7 +326,9 @@ void SceneGraphRenderer::drawLayer(const std_msgs::msg::Header& header, } const auto node_ns = MarkerNamespaces::layerNodeNamespace(layer.id); - tracker_.add(makeLayerNodeMarkers(header, info, layer, node_ns), msg); + if (info.config.nodes.draw) { + tracker_.add(makeLayerNodeMarkers(header, info, layer, node_ns), msg); + } if (info.config.text.draw) { if (info.config.text.draw_layer) { @@ -351,6 +368,18 @@ void SceneGraphRenderer::drawLayer(const std_msgs::msg::Header& header, const auto edge_ns = MarkerNamespaces::layerEdgeNamespace(layer.id); tracker_.add(makeLayerEdgeMarkers(header, info, layer, edge_ns), msg); + + // dispatch drawing to layer plugins + auto plugins = layer_plugins_.find(layer.id); + if (plugins != layer_plugins_.end()) { + for (const auto& plugin : plugins->second) { + if (!plugin) { + continue; + } + + plugin->draw(header, info, layer, mesh, msg, tracker_); + } + } } LayerConfig SceneGraphRenderer::getLayerConfig(spark_dsg::LayerKey key) const { diff --git a/hydra_visualizer/src/utils/layer_key_selector.cpp b/hydra_visualizer/src/utils/layer_key_selector.cpp index 65ae45f..68a9f1f 100644 --- a/hydra_visualizer/src/utils/layer_key_selector.cpp +++ b/hydra_visualizer/src/utils/layer_key_selector.cpp @@ -126,4 +126,28 @@ void SelectorConversion::fromIntermediate(const std::string& intermediate, value = *parsed; } +std::string LayerKeyConversion::toIntermediate(const LayerKey& value, std::string&) { + return LayerKeySelector{value}.str(); +} + +void LayerKeyConversion::fromIntermediate(const std::string& intermediate, + LayerKey& value, + std::string& error) { + std::regex re(R"((\d+)p(\d+)$|(\d+)$)"); + std::smatch match; + if (!std::regex_match(intermediate, match, re)) { + error = "Invalid layer key '" + intermediate + "'!"; + return; + } + + CHECK_EQ(match.size(), 4); + if (!match.str(1).empty()) { + // layer and partition + const auto part_id = static_cast(std::stoi(match.str(2))); + value = LayerKey{std::stol(match.str(1)), part_id}; + } else { + value = LayerKey{std::stol(match.str(3))}; + } +} + } // namespace hydra