diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index 376fa3e1..65ce11d8 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/hydra_visualizer/config/visualizer_config.yaml @@ -1,4 +1,23 @@ --- +plugins: + mesh: + type: MeshPlugin + coloring: + type: SemanticMeshColoring + places_basis_points: + type: BasisPointPlugin + show_voxblox_connections: false + draw_basis_points: true + basis_point_scale: 0.1 + places_node_scale: 0.1 + khronos_objects: + type: KhronosObjectPlugin + dynamic_color_mode: CONSTANT # ID, SEMANTIC, CONSTANT + places_traversability: + type: TraversabilityPlugin + agent_poses: + type: PosePlugin + num_to_skip: 0 renderer: layer_z_step: 5.5 # unit separation between layers collapse_layers: false # whether or not to apply offsets to each of the layers diff --git a/hydra_visualizer/config/visualizer_plugins.yaml b/hydra_visualizer/config/visualizer_plugins.yaml deleted file mode 100644 index 5980d443..00000000 --- a/hydra_visualizer/config/visualizer_plugins.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -plugins: - mesh: - type: MeshPlugin - coloring: - type: SemanticMeshColoring - places_basis_points: - type: BasisPointPlugin - show_voxblox_connections: false - draw_basis_points: true - basis_point_scale: 0.1 - places_node_scale: 0.1 - khronos_objects: - type: KhronosObjectPlugin - dynamic_color_mode: CONSTANT # ID, SEMANTIC, CONSTANT - places_traversability: - type: TraversabilityPlugin - agent_poses: - type: PosePlugin - num_to_skip: 0 -# dynamic reconfigure settings -places_basis_points: - graph: - visualize: true diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h index 29c0fffa..f5c5b0da 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h @@ -70,6 +70,8 @@ class BasisPointPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: void fillMarkers(const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h index d50964e9..6097b579 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h @@ -66,6 +66,8 @@ class FootprintPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: rclcpp::Publisher::SharedPtr pub_; MarkerTracker tracker_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h index f959fa9f..80b5aa91 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h @@ -135,6 +135,8 @@ class KhronosObjectPlugin : public VisualizerPlugin { const spark_dsg::DynamicSceneGraph& graph) override; void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: // Helper functions. void drawDynamicObjects(const Config& config, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h index 1898fc0f..bd96d9ef 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h @@ -59,6 +59,8 @@ struct LayerPlugin { virtual void clearChangeFlag() { has_change_ = false; } + virtual YAML::Node dumpConfig() const { return YAML::Node(); } + protected: bool has_change_ = false; }; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h index a68f0607..6f2b4357 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h @@ -63,6 +63,8 @@ class MeshPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: config::DynamicConfig config_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h index ad98ea1e..f21d8e4b 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h @@ -67,6 +67,8 @@ class MeshPointPlugin : public LayerPlugin { visualization_msgs::msg::MarkerArray& msg, MarkerTracker& tracker) override; + YAML::Node dumpConfig() const override; + private: std::string ns_; config::DynamicConfig config_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h index 0ecaac34..dc0bc3f4 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h @@ -64,6 +64,8 @@ class PlacesFreespacePlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: void fillMarkers(const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h index 4439de25..367ef644 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h @@ -60,6 +60,8 @@ class PosePlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: size_t num_received_; rclcpp::Publisher::SharedPtr pub_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h index aed858bf..97ba5ec4 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h @@ -43,8 +43,14 @@ namespace hydra { class RegionGrowingBoundaryPlugin : public LayerPlugin { public: struct Config { - //! line width of the boundary markers + //! Use node color instead of boundary type + bool use_node_color = false; + //! Line width of the boundary markers float line_width = 0.07f; + //! Alpha of polygon fill + float fill_alpha = 0.5f; + //! Fill boundaries + bool fill_boundaries = false; //! Colors representing each traversability state. std::vector colors{spark_dsg::Color::gray(), // Unknown spark_dsg::Color::blue(), // Traversable @@ -63,6 +69,8 @@ class RegionGrowingBoundaryPlugin : public LayerPlugin { visualization_msgs::msg::MarkerArray& msg, MarkerTracker& tracker) override; + YAML::Node dumpConfig() const override; + protected: const std::string ns_; config::DynamicConfig config_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h index 40201d78..ac020ea4 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h @@ -75,6 +75,8 @@ class TraversabilityPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: void fillMarkers(const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h index a47fcef4..bfedfdc0 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once #include +#include #include @@ -57,6 +58,8 @@ class VisualizerPlugin { virtual void clearChangeFlag() { has_change_ = false; } + virtual YAML::Node dumpConfig() const { return YAML::Node(); } + const std::string name; protected: diff --git a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h index f56424e2..0887cc42 100644 --- a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h +++ b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h @@ -106,6 +106,8 @@ class SceneGraphRenderer { virtual void clearChangeFlag(); + YAML::Node dumpConfig() const; + protected: virtual void setConfigs(const spark_dsg::DynamicSceneGraph& graph) const; @@ -138,7 +140,13 @@ class SceneGraphRenderer { mutable std::atomic has_change_; mutable std::map layer_infos_; mutable std::map> layers_; - mutable std::map> interlayer_edges_; + + struct EdgeConfigInfo { + spark_dsg::LayerKey parent; + spark_dsg::LayerKey child; + std::unique_ptr config; + }; + mutable std::map interlayer_edges_; }; void declare_config(SceneGraphRenderer::LayerPluginsConfig& config); diff --git a/hydra_visualizer/include/hydra_visualizer/visualizer_node.h b/hydra_visualizer/include/hydra_visualizer/visualizer_node.h index bce207b0..e95a584a 100644 --- a/hydra_visualizer/include/hydra_visualizer/visualizer_node.h +++ b/hydra_visualizer/include/hydra_visualizer/visualizer_node.h @@ -38,6 +38,7 @@ #include #include +#include #include #include "hydra_visualizer/io/graph_wrapper.h" @@ -76,6 +77,8 @@ class DsgVisualizer { private: void spinOnce(bool force = false); + void saveConfigs(const std::filesystem::path& output); + ianvs::NodeHandle nh_; rclcpp::TimerBase::SharedPtr loop_timer_; @@ -85,6 +88,7 @@ class DsgVisualizer { const config::RosDynamicConfigServer server_; rclcpp::Service::SharedPtr redraw_service_; rclcpp::Service::SharedPtr reset_service_; + rclcpp::Subscription::SharedPtr save_sub_; }; void declare_config(DsgVisualizer::Config& config); diff --git a/hydra_visualizer/launch/static_visualizer.launch.yaml b/hydra_visualizer/launch/static_visualizer.launch.yaml index e15d0a37..e4d8600d 100644 --- a/hydra_visualizer/launch/static_visualizer.launch.yaml +++ b/hydra_visualizer/launch/static_visualizer.launch.yaml @@ -5,9 +5,8 @@ launch: - arg: {name: scene_graph, default: '', description: filepath for scene graph to show} - arg: {name: visualizer_frame, default: map, description: frame_id for visualizations} - arg: {name: visualizer_ns, default: hydra_dsg_visualizer, description: visualizer namespace} - - arg: {name: visualizer_config_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml} - - arg: {name: visualizer_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_plugins.yaml} - - arg: {name: external_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml} + - arg: {name: visualizer_config, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml} + - arg: {name: external_plugins, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml} # visualizer node and control for launching - node: pkg: hydra_visualizer @@ -16,9 +15,8 @@ launch: launch-prefix: $(if $(var debug) 'gdbserver localhost:3000') on_exit: shutdown args: > - --config-utilities-file $(var visualizer_config_path) - --config-utilities-file $(var visualizer_plugins_path) - --config-utilities-file $(var external_plugins_path) + --config-utilities-file $(var visualizer_config) + --config-utilities-file $(var external_plugins) --config-utilities-yaml {glog_level: 0, glog_verbosity: $(var verbosity)} --config-utilities-yaml {graph: {type: GraphFromFile, frame_id: $(var visualizer_frame), filepath: $(var scene_graph)}} # rviz node and control for launching diff --git a/hydra_visualizer/launch/streaming_visualizer.launch.yaml b/hydra_visualizer/launch/streaming_visualizer.launch.yaml index 8d55c5c1..c588c74e 100644 --- a/hydra_visualizer/launch/streaming_visualizer.launch.yaml +++ b/hydra_visualizer/launch/streaming_visualizer.launch.yaml @@ -6,7 +6,6 @@ launch: # visualizer configuration - arg: {name: visualizer_ns, default: hydra_visualizer} - arg: {name: visualizer_config_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml} - - arg: {name: visualizer_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_plugins.yaml} - arg: {name: external_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml} # communication - arg: {name: use_zmq, default: 'false', description: use zmq to receive scene graphs} @@ -23,7 +22,6 @@ launch: on_exit: shutdown args: > --config-utilities-file $(var visualizer_config_path) - --config-utilities-file $(var visualizer_plugins_path) --config-utilities-file $(var external_plugins_path) --config-utilities-yaml {glog_level: 0, glog_verbosity: $(var verbosity)} --config-utilities-yaml {graph: {type: $(if $(var use_zmq) GraphFromZmq GraphFromRos), url: $(var zmq_url), frame_id: $(var visualizer_frame)}} diff --git a/hydra_visualizer/rviz/static_visualizer.rviz b/hydra_visualizer/rviz/static_visualizer.rviz index d9c68da0..5943b24a 100644 --- a/hydra_visualizer/rviz/static_visualizer.rviz +++ b/hydra_visualizer/rviz/static_visualizer.rviz @@ -9,7 +9,7 @@ Panels: - /Mesh1/Topic1 - /Scene Graph1/Topic1 Splitter Ratio: 0.5 - Tree Height: 555 + Tree Height: 1065 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -72,7 +72,26 @@ Visualization Manager: Enabled: true Name: Scene Graph Namespaces: - {} + interlayer_edges_3[2]_2: true + interlayer_edges_3[2]_4: true + interlayer_edges_4_3: true + layer2_bounding_boxes: true + layer2_nodes: true + layer2_text: true + layer2p97_edges: true + layer2p97_nodes: true + layer2p97_text: true + layer3_edges: true + layer3_nodes: true + layer3p1_edges: true + layer3p1_nodes: true + layer3p1_polygon_boundaries: true + layer3p2_edges: true + layer3p2_nodes: true + layer3p2_region_growing_boundaries: true + layer4_edges: true + layer4_nodes: true + layer4_text: true Topic: Depth: 5 Durability Policy: Transient Local @@ -126,16 +145,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 31.058483123779297 + Distance: 108.03848266601562 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -6.118514060974121 - Y: -0.15275907516479492 - Z: 1.9664970636367798 + X: 4.692053318023682 + Y: 5.916086196899414 + Z: -5.913562774658203 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -149,10 +168,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 846 + Height: 1356 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000004b2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004b2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006ac0000003efc0100000002fb0000000800540069006d00650100000000000006ac0000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000550000004b200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -161,6 +180,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1200 - X: 60 - Y: 60 + Width: 1708 + X: 1724 + Y: 76 diff --git a/hydra_visualizer/src/plugins/basis_point_plugin.cpp b/hydra_visualizer/src/plugins/basis_point_plugin.cpp index 1ec5cbec..ff0f1e84 100644 --- a/hydra_visualizer/src/plugins/basis_point_plugin.cpp +++ b/hydra_visualizer/src/plugins/basis_point_plugin.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -244,4 +245,10 @@ void BasisPointPlugin::drawBasisPoints(const Config& config, tracker_.add(marker, msg); } +YAML::Node BasisPointPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "BasisPointPlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/footprint_plugin.cpp b/hydra_visualizer/src/plugins/footprint_plugin.cpp index 84284e59..6268ca79 100644 --- a/hydra_visualizer/src/plugins/footprint_plugin.cpp +++ b/hydra_visualizer/src/plugins/footprint_plugin.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -56,7 +57,6 @@ static const auto registration_ = std::string>("FootprintPlugin"); } -using spark_dsg::DsgLayers; using spark_dsg::DynamicSceneGraph; using spark_dsg::LayerId; using spark_dsg::PlaceNodeAttributes; @@ -174,4 +174,10 @@ void FootprintPlugin::reset(const std_msgs::msg::Header& header) { } } +YAML::Node FootprintPlugin::dumpConfig() const { + auto root = config::toYaml(config); + root["type"] = "FootprintPlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp index bdddb067..57d9deb9 100644 --- a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp +++ b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp @@ -75,6 +75,7 @@ #include #include +#include #include #include #include @@ -313,4 +314,10 @@ Color KhronosObjectPlugin::getDynamicColor(const Config& config, } } +YAML::Node KhronosObjectPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "KhronosObjectPlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index 7d4ff981..322b097b 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_plugin.cpp @@ -35,6 +35,7 @@ #include "hydra_visualizer/plugins/mesh_plugin.h" #include +#include #include #include @@ -97,4 +98,10 @@ std::string MeshPlugin::getMsgNamespace() const { return "robot0/dsg_mesh"; } +YAML::Node MeshPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "MeshPlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/mesh_point_plugin.cpp b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp index 974ad2ca..4f8b2ad8 100644 --- a/hydra_visualizer/src/plugins/mesh_point_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp @@ -1,6 +1,7 @@ #include "hydra_visualizer/plugins/mesh_point_plugin.h" #include +#include #include "hydra_visualizer/color/colormap_utilities.h" @@ -131,4 +132,10 @@ void MeshPointPlugin::draw(const std_msgs::msg::Header& header, tracker.add(marker, msg); } +YAML::Node MeshPointPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "MeshPointPlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/places_freespace_plugin.cpp b/hydra_visualizer/src/plugins/places_freespace_plugin.cpp index 626a6141..95b3502d 100644 --- a/hydra_visualizer/src/plugins/places_freespace_plugin.cpp +++ b/hydra_visualizer/src/plugins/places_freespace_plugin.cpp @@ -34,7 +34,8 @@ * -------------------------------------------------------------------------- */ #include "hydra_visualizer/plugins/places_freespace_plugin.h" -#include +#include +#include #include #include #include @@ -62,8 +63,6 @@ using spark_dsg::DsgLayers; using spark_dsg::DynamicSceneGraph; using spark_dsg::PlaceNodeAttributes; using spark_dsg::SceneGraphLayer; -using spark_dsg::SceneGraphNode; -using spark_dsg::SemanticNodeAttributes; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -157,4 +156,10 @@ void PlacesFreespacePlugin::drawSpheres(const Config& config, } } +YAML::Node PlacesFreespacePlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "PlacesFreespacePlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/pose_plugin.cpp b/hydra_visualizer/src/plugins/pose_plugin.cpp index 11d85b0e..83f41c02 100644 --- a/hydra_visualizer/src/plugins/pose_plugin.cpp +++ b/hydra_visualizer/src/plugins/pose_plugin.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -124,4 +125,10 @@ void PosePlugin::reset(const std_msgs::msg::Header& header) { pub_->publish(msg); } +YAML::Node PosePlugin::dumpConfig() const { + auto root = config::toYaml(config); + root["type"] = "PosePlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp index 8dab32ee..a992a10f 100644 --- a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp +++ b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp @@ -35,12 +35,14 @@ #include "hydra_visualizer/plugins/region_growing_boundary_plugin.h" #include +#include #include #include #include #include "hydra_visualizer/color/color_parsing.h" +#include "hydra_visualizer/utils/polygon_utilities.h" namespace hydra { namespace { @@ -52,8 +54,9 @@ static const auto registration_ = std::string>("RegionGrowingBoundaryPlugin"); std_msgs::msg::ColorRGBA makeBoundaryColor(const std::vector& colors, - spark_dsg::TraversabilityState state) { - return visualizer::makeColorMsg(colors.at(static_cast(state))); + spark_dsg::TraversabilityState state, + float alpha) { + return visualizer::makeColorMsg(colors.at(static_cast(state)), alpha); } } // namespace @@ -63,16 +66,21 @@ using visualization_msgs::msg::Marker; void declare_config(RegionGrowingBoundaryPlugin::Config& config) { using namespace config; name("RegionGrowingBoundaryPlugin::Config"); + field(config.use_node_color, "use_node_color"); field(config.colors, "colors"); field(config.line_width, "line_width"); + field(config.fill_alpha, "fill_alpha"); + field(config.fill_boundaries, "fill_boundaries"); checkCondition(config.colors.size() == 4, "colors.size() must be 4"); check(config.line_width, GT, 0.0f, "line_width"); + checkInRange(config.fill_alpha, 0.0f, 1.0f, "fill_alpha", false); } RegionGrowingBoundaryPlugin::RegionGrowingBoundaryPlugin(const Config& config, const std::string& ns) : ns_(ns), - config_(ns + "_mesh_point_plugin", config, [this]() { has_change_ = true; }) {} + config_(ns + "_region_growing_plugin", config, [this]() { has_change_ = true; }) { +} void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, const visualizer::LayerInfo& info, @@ -92,33 +100,73 @@ void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, marker.scale.x = config.line_width; marker.scale.y = config.line_width; marker.scale.z = config.line_width; + + Marker fill_marker; + fill_marker.header = header; + fill_marker.ns = ns_ + "_region_growing_polygons"; + fill_marker.id = 0; + fill_marker.type = Marker::TRIANGLE_LIST; + fill_marker.action = Marker::ADD; + fill_marker.pose.orientation.w = 1.0; + fill_marker.color.a = config.fill_alpha; + fill_marker.scale.x = 1.0; + fill_marker.scale.y = 1.0; + fill_marker.scale.z = 1.0; + for (const auto& [node_id, node] : layer.nodes()) { if (info.filter && !info.filter(*node)) { continue; } auto attrs = node->tryAttributes(); - if (!attrs) { + if (!attrs || attrs->radii.empty()) { continue; } + Eigen::MatrixXd points(3, attrs->radii.size()); + const auto color = + visualizer::makeColorMsg(info.node_color(*node), info.config.nodes.alpha); for (size_t i = 1; i <= attrs->radii.size(); ++i) { const auto start_idx = i - 1; const auto end_idx = i % attrs->radii.size(); auto start = attrs->getBoundaryPoint(start_idx); start.z() += info.z_offset; + points.col(start_idx) = start; + tf2::convert(start, marker.points.emplace_back()); auto end = attrs->getBoundaryPoint(end_idx); end.z() += info.z_offset; tf2::convert(end, marker.points.emplace_back()); - marker.colors.emplace_back( - makeBoundaryColor(config.colors, attrs->states[start_idx])); - marker.colors.emplace_back( - makeBoundaryColor(config.colors, attrs->states[end_idx])); + if (config.use_node_color) { + marker.colors.emplace_back(color); + marker.colors.emplace_back(color); + } else { + const auto start_color = makeBoundaryColor( + config.colors, attrs->states[start_idx], info.config.nodes.alpha); + const auto end_color = makeBoundaryColor( + config.colors, attrs->states[end_idx], info.config.nodes.alpha); + marker.colors.emplace_back(start_color); + marker.colors.emplace_back(end_color); + } + } + + if (config.fill_boundaries) { + auto mesh_color = color; + mesh_color.a = config.fill_alpha; + makeFilledPolygon(points, mesh_color, fill_marker); } } tracker.add(marker, msg); + if (config.fill_boundaries) { + tracker.add(fill_marker, msg); + } +} + +YAML::Node RegionGrowingBoundaryPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "RegionGrowingBoundaryPlugin"; + return root; } } // namespace hydra diff --git a/hydra_visualizer/src/plugins/traversability_plugin.cpp b/hydra_visualizer/src/plugins/traversability_plugin.cpp index 150f187d..4ba4e61f 100644 --- a/hydra_visualizer/src/plugins/traversability_plugin.cpp +++ b/hydra_visualizer/src/plugins/traversability_plugin.cpp @@ -35,6 +35,7 @@ #include "hydra_visualizer/plugins/traversability_plugin.h" #include +#include #include #include #include @@ -236,4 +237,10 @@ void TraversabilityPlugin::addBoundaryPoint(const Config& config, } } +YAML::Node TraversabilityPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "TraversabilityPlugin"; + return root; +} + } // namespace hydra diff --git a/hydra_visualizer/src/scene_graph_renderer.cpp b/hydra_visualizer/src/scene_graph_renderer.cpp index f4456b7b..8bc4073f 100644 --- a/hydra_visualizer/src/scene_graph_renderer.cpp +++ b/hydra_visualizer/src/scene_graph_renderer.cpp @@ -35,13 +35,13 @@ #include "hydra_visualizer/scene_graph_renderer.h" #include +#include #include #include #include #include #include -#include #include #include "hydra_visualizer/color/colormap_utilities.h" @@ -175,9 +175,32 @@ void SceneGraphRenderer::reset(const std_msgs::msg::Header& header) { } } -bool SceneGraphRenderer::hasChange() const { return has_change_; } +bool SceneGraphRenderer::hasChange() const { + if (has_change_) { + return true; + } + + for (const auto& [key, plugins] : layer_plugins_) { + for (const auto& plugin : plugins) { + if (plugin && plugin->hasChange()) { + return true; + } + } + } + + return false; +} -void SceneGraphRenderer::clearChangeFlag() { has_change_ = false; } +void SceneGraphRenderer::clearChangeFlag() { + has_change_ = false; + for (const auto& [key, plugins] : layer_plugins_) { + for (const auto& plugin : plugins) { + if (plugin) { + plugin->clearChangeFlag(); + } + } + } +} void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, const DynamicSceneGraph& graph) const { @@ -204,6 +227,44 @@ void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, } } +YAML::Node SceneGraphRenderer::dumpConfig() const { + YAML::Node layers(YAML::NodeType::Map); + for (const auto& [key, wrapper] : layers_) { + layers[LayerKeySelector{key}.str()] = config::toYaml(wrapper->get()); + } + + YAML::Node layer_plugins(YAML::NodeType::Sequence); + for (const auto& [key, plugins] : layer_plugins_) { + YAML::Node plugin_configs(YAML::NodeType::Sequence); + for (const auto& plugin : plugins) { + if (!plugin) { + continue; + } + + plugin_configs.push_back(plugin->dumpConfig()); + } + + YAML::Node layer_info; + layer_info["layer"] = LayerKeySelector{key}.str(); + layer_info["plugins"] = plugin_configs; + layer_plugins.push_back(layer_info); + } + + YAML::Node interlayer_edge_configs(YAML::NodeType::Sequence); + for (const auto& [_, info] : interlayer_edges_) { + YAML::Node this_config = config::toYaml(info.config->get()); + this_config["from"] = LayerKeySelector{info.parent}.str(); + this_config["to"] = LayerKeySelector{info.child}.str(); + interlayer_edge_configs.push_back(this_config); + } + + YAML::Node root = config::toYaml(graph_config_.get()); + root["layers"] = layers; + root["layer_plugins"] = layer_plugins; + root["interlayer_edges"] = interlayer_edge_configs; + return root; +} + InterlayerEdgeConfig SceneGraphRenderer::getInterlayerEdgeConfig(LayerKey parent, LayerKey child) const { const auto name = keyToLayerName(parent) + "_to_" + keyToLayerName(child); @@ -228,10 +289,12 @@ InterlayerEdgeConfig SceneGraphRenderer::getInterlayerEdgeConfig(LayerKey parent const auto ns = "scene_graph_interlayer_" + name; auto wrapper = std::make_unique(ns, config); wrapper->setCallback([this]() { has_change_ = true; }); - iter = interlayer_edges_.emplace(name, std::move(wrapper)).first; + iter = interlayer_edges_ + .emplace(name, EdgeConfigInfo{parent, child, std::move(wrapper)}) + .first; } - return iter->second->get(); + return iter->second.config->get(); } void SceneGraphRenderer::drawInterlayerEdges(const std_msgs::msg::Header& header, diff --git a/hydra_visualizer/src/visualizer_node.cpp b/hydra_visualizer/src/visualizer_node.cpp index bed30aa7..15a54ed5 100644 --- a/hydra_visualizer/src/visualizer_node.cpp +++ b/hydra_visualizer/src/visualizer_node.cpp @@ -39,8 +39,12 @@ #include #include +#include + namespace hydra { +using std_msgs::msg::String; + void declare_config(DsgVisualizer::Config& config) { using namespace config; name("HydraVisualizerConfig"); @@ -67,6 +71,8 @@ DsgVisualizer::DsgVisualizer(const Config& config, ianvs::NodeHandle nh) "reset", [this](const std_srvs::srv::Empty::Request::SharedPtr&, std_srvs::srv::Empty::Response::SharedPtr) { reset(); }); + save_sub_ = nh_.create_subscription( + "save_config", 1, [this](const String& msg) { saveConfigs(msg.data); }); } void DsgVisualizer::start() { @@ -141,4 +147,23 @@ void DsgVisualizer::spinOnce(bool force) { } } +void DsgVisualizer::saveConfigs(const std::filesystem::path& output) { + YAML::Node plugins(YAML::NodeType::Map); + for (const auto& plugin : plugins_) { + if (!plugin) { + continue; + } + + plugins[plugin->name] = plugin->dumpConfig(); + } + + YAML::Node root; + root["renderer"] = renderer_->dumpConfig(); + root["plugins"] = plugins; + + std::ofstream fout(output); + fout << root; + LOG(INFO) << "Saved config to " << output; +} + } // namespace hydra