diff --git a/hydra_ros/launch/lukas_region_trav.launch.yaml b/hydra_ros/launch/lukas_region_trav.launch.yaml new file mode 100644 index 0000000..eab4d9a --- /dev/null +++ b/hydra_ros/launch/lukas_region_trav.launch.yaml @@ -0,0 +1,60 @@ +--- +launch: + - arg: { name: start_visualizer, default: "true" } + - arg: { name: scene, default: office, description: uhumans2 scene name } + - arg: { name: use_gt_semantics, default: "true" } + - arg: { name: use_openset_places, default: "false" } + - arg: { name: labelspace, default: $(if $(var use_gt_semantics) uhumans2_$(var scene) ade20k_full) } + - arg: { name: log_prefix, default: uhumans2_$(var scene) } + - arg: { name: label_remap_path, default: $(find-pkg-share hydra)/config/label_remaps/uhumans2_$(var scene).yaml } + - set_remap: { from: hydra/input/left_cam/depth_registered/image_rect, to: /tesse/depth_cam/mono/image_raw } + - set_remap: { from: hydra/input/left_cam/rgb/image_raw, to: /tesse/left_cam/rgb/image_raw } + - set_remap: { from: hydra/input/left_cam/rgb/camera_info, to: /tesse/left_cam/camera_info } + - set_remap: + { + from: hydra/input/left_cam/semantic/image_raw, + to: $(if $(var use_gt_semantics) /tesse/seg_cam/converted/image_raw /tesse/left_cam/semantic/image_raw), + } + - set_remap: { from: hydra/input/left_cam/feature, to: /tesse/left_cam/semantic/feature } + - let: { name: extra_yaml_gt, value: "{semantic_label_remap_filepath: $(var label_remap_path)}" } + - let: { name: extra_yaml_no_gt, value: "{}" } + - group: + - push_ros_namespace: { namespace: tesse/left_cam } + - set_remap: { from: color/image_raw, to: rgb/image_raw } + - include: + if: $(not $(var use_gt_semantics)) + file: $(find-pkg-share semantic_inference_ros)/launch/closed_set.launch.yaml + arg: + - { name: labelspace_name, value: $(var labelspace) } + - include: + if: $(var use_openset_places) + file: $(find-pkg-share semantic_inference_ros)/launch/image_embedding_node.launch.yaml + arg: + - { name: min_period_s, value: "0.2" } + - include: + file: $(find-pkg-share hydra_ros)/launch/hydra.launch.yaml + arg: + - { name: dataset, value: uhumans2 } + - { name: labelspace, value: $(var labelspace) } + - { name: log_prefix, value: $(var log_prefix) } + - { name: sensor_frame, value: left_cam } + - { name: robot_frame, value: base_link_gt } + - { name: odom_frame, value: world } + - { name: map_frame, value: map } + - { name: lcd_config_path, value: $(find-pkg-share hydra)/config/lcd/uhumans2.yaml } + - { name: extra_yaml, value: $(if $(var use_gt_semantics) $(var extra_yaml_gt) $(var extra_yaml_no_gt)) } + - { name: hydra_config_path, value: $(find-pkg-share hydra)/config/datasets/lukas_trav.yaml } + - { name: rviz_path, value: $(find-pkg-share hydra_visualizer)/rviz/lukas_trav.rviz } + + - include: + file: $(find-pkg-share khronos_ros)/launch/datasets/play_uhumans.launch.yaml + arg: + - { name: bag_file, value: /home/lukas/data/khronos/tesse_cd_office.bag } + - { name: use_gt_frame, value: "true" } + - { name: play_rate, value: "1.0" } + + - node: + pkg: tf2_ros + name: tf_world_to_map + exec: static_transform_publisher + args: --frame-id world --child-frame-id map diff --git a/hydra_ros/src/frontend/traversability_visualizer.cpp b/hydra_ros/src/frontend/traversability_visualizer.cpp index 968edcc..85bd0ff 100644 --- a/hydra_ros/src/frontend/traversability_visualizer.cpp +++ b/hydra_ros/src/frontend/traversability_visualizer.cpp @@ -70,7 +70,7 @@ TraversabilityVisualizer::TraversabilityVisualizer(const Config& config) config::checkValid(config), [this]() { onConfigUpdate(); }), nh_(ianvs::NodeHandle::this_node(config.ns)), - layer_pub_(nh_.create_publisher("layer", 10)) { + layer_pub_(nh_.create_publisher("layer", 100)) { onConfigUpdate(); } diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index b3ba352..d377cfd 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/hydra_visualizer/config/visualizer_config.yaml @@ -9,7 +9,7 @@ renderer: nodes: { 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 } - edges: { interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.9 } + edges: { interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.5 } 3: z_offset_scale: 3.0 visualize: true @@ -57,12 +57,13 @@ renderer: 3: z_offset_scale: 3.0 visualize: true - nodes: { scale: 0.2, color: { type: LabelColorAdapter }, alpha: 0.9, use_sphere: true } + nodes: { scale: 0.3, color: { type: StatusColorAdapter }, alpha: 0.9, use_sphere: true } boundaries: { draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0 } + text: { draw: true, collapse: true, adapter: { type: IdTextAdapter }, height: 0.5, scale: 0.45 } edges: - scale: 0.01 - alpha: 0.5 - color: { type: UniformEdgeColorAdapter } + scale: 0.1 + alpha: 0.8 + color: { type: TraversabilityEdgeColorAdapter } interlayer_use_source: false interlayer_scale: 0.08 interlayer_alpha: 0.4 diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h index f08a1eb..ea5cd32 100644 --- a/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h +++ b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h @@ -126,6 +126,30 @@ struct ValueEdgeColorAdapter : EdgeColorAdapter { void declare_config(ValueEdgeColorAdapter::Config& config); +struct TraversabilityEdgeColorAdapter : EdgeColorAdapter { + struct Config { + visualizer::RangeColormap::Config colormap{ + config::VirtualConfig{ + visualizer::QualityPalette::Config()}}; + spark_dsg::Color active_color = spark_dsg::Color::pink(); + spark_dsg::Color backend_color = spark_dsg::Color::blue(); + } const config; + + explicit TraversabilityEdgeColorAdapter(const Config& config); + void setGraph(const spark_dsg::DynamicSceneGraph& graph, + spark_dsg::LayerId layer) override; + EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph, + const spark_dsg::SceneGraphEdge& edge) const override; + + private: + double min_value_; + double max_value_; + const visualizer::RangeColormap colormap_; + REGISTER_COLOR_ADAPTER(TraversabilityEdgeColorAdapter); +}; + +void declare_config(TraversabilityEdgeColorAdapter::Config& config); + #undef REGISTER_COLOR_ADAPTER } // namespace hydra diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h index 74c0736..16d4c58 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h @@ -58,7 +58,7 @@ class TraversabilityPlugin : public VisualizerPlugin { float slice_height = 2.0f; //! line width of the boundary markers - float line_width = 0.05f; + float line_width = 0.07f; }; TraversabilityPlugin(const Config& config, @@ -82,6 +82,14 @@ class TraversabilityPlugin : public VisualizerPlugin { const spark_dsg::SceneGraphLayer& layer, visualization_msgs::msg::MarkerArray& msg) const; + void drawBlockBoundary(const Config& config, + const spark_dsg::TraversabilityNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const; + + void drawRegionBoundary(const Config& config, + const spark_dsg::TravNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const; + void addBoundaryPoint(const Config& config, visualization_msgs::msg::Marker& marker, const Eigen::Vector3d& point, @@ -91,6 +99,14 @@ class TraversabilityPlugin : public VisualizerPlugin { config::DynamicConfig config_; rclcpp::Publisher::SharedPtr pub_; mutable MarkerTracker tracker_; + + // Start and stop indices for the corner points of the boundary along the state + // direction. + inline static const std::array, 4> state_pairs_ = { + std::make_pair(1, 0), + std::make_pair(1, 2), + std::make_pair(2, 3), + std::make_pair(0, 3)}; }; void declare_config(TraversabilityPlugin::Config& config); diff --git a/hydra_visualizer/launch/static_visualizer.launch.yaml b/hydra_visualizer/launch/static_visualizer.launch.yaml index 7d25e0f..2cf39de 100644 --- a/hydra_visualizer/launch/static_visualizer.launch.yaml +++ b/hydra_visualizer/launch/static_visualizer.launch.yaml @@ -1,14 +1,13 @@ --- launch: - - arg: {name: verbosity, default: '0', description: visualizer verbosity} - - arg: {name: debug, default: 'false', description: launch with gdb server} - - 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: use_color_adapter, default: 'false', description: Start visualizer using mesh color adapter} + - arg: { name: verbosity, default: "0", description: visualizer verbosity } + - arg: { name: debug, default: "false", description: launch with gdb server } + - 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 } # visualizer node and control for launching - node: pkg: hydra_visualizer @@ -22,11 +21,16 @@ launch: --config-utilities-file $(var external_plugins_path) --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)}} - --config-utilities-yaml {plugins: {mesh: {use_color_adapter: $(var use_color_adapter)}}} # rviz node and control for launching - - arg: {name: start_rviz, default: 'true', description: automatically start rviz} - - arg: {name: rviz_path, default: $(find-pkg-share hydra_visualizer)/rviz/static_visualizer.rviz, description: rviz file to load} - - node: {if: $(var start_rviz), name: rviz, pkg: rviz2, exec: rviz2, args: -d $(var rviz_path)} + - arg: { name: start_rviz, default: "true", description: automatically start rviz } + - arg: + { + name: rviz_path, + default: $(find-pkg-share hydra_visualizer)/rviz/static_visualizer.rviz, + description: rviz file to load, + } + - node: { if: $(var start_rviz), name: rviz, pkg: rviz2, exec: rviz2, args: -d $(var rviz_path) } # reconfiguration of dynamic configurations - - arg: {name: start_config_gui, default: 'true', description: automatically start config_utilities configuration GUI} - - node: {if: $(var start_config_gui), name: dynamic_config_gui, pkg: config_utilities_ros, exec: gui} + - arg: + { name: start_config_gui, default: "true", description: automatically start config_utilities configuration GUI } + - node: { if: $(var start_config_gui), name: dynamic_config_gui, pkg: config_utilities_ros, exec: gui } diff --git a/hydra_visualizer/launch/streaming_visualizer.launch.yaml b/hydra_visualizer/launch/streaming_visualizer.launch.yaml index 36a56bc..fd58e39 100644 --- a/hydra_visualizer/launch/streaming_visualizer.launch.yaml +++ b/hydra_visualizer/launch/streaming_visualizer.launch.yaml @@ -1,20 +1,19 @@ --- launch: # development args - - arg: {name: verbosity, default: '0'} - - arg: {name: debug, default: 'false'} + - arg: { name: verbosity, default: "0" } + - arg: { name: debug, default: "false" } # 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} - - arg: {name: use_color_adapter, default: 'false', description: Start visualizer using mesh color adapter} + - 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} - - arg: {name: visualizer_frame, default: map, description: frame ID for zmq to use} - - arg: {name: zmq_url, default: 'tcp://127.0.0.1:8001', description: full zmq url} + - arg: { name: use_zmq, default: "false", description: use zmq to receive scene graphs } + - arg: { name: visualizer_frame, default: map, description: frame ID for zmq to use } + - arg: { name: zmq_url, default: "tcp://127.0.0.1:8001", description: full zmq url } # visualizer node and control for launching - - arg: {name: start_visualizer, default: 'true'} + - arg: { name: start_visualizer, default: "true" } - node: if: $(var start_visualizer) pkg: hydra_visualizer @@ -28,8 +27,12 @@ launch: --config-utilities-file $(var external_plugins_path) --config-utilities-yaml {glog_level: 1, glog_verbosity: $(var verbosity)} --config-utilities-yaml {graph: {type: $(if $(var use_zmq) GraphFromZmq GraphFromRos), url: $(var zmq_url), frame_id: $(var visualizer_frame)}} - --config-utilities-yaml {plugins: {mesh: {use_color_adapter: $(var use_color_adapter)}}} # rviz node and control for launching - - arg: {name: start_rviz, default: 'true', description: automatically start rviz} - - arg: {name: rviz_path, default: $(find-pkg-share hydra_visualizer)/rviz/streaming_visualizer.rviz, description: rviz file to load} - - node: {if: $(var start_rviz), name: rviz, pkg: rviz2, exec: rviz2, args: -d $(var rviz_path)} + - arg: { name: start_rviz, default: "true", description: automatically start rviz } + - arg: + { + name: rviz_path, + default: $(find-pkg-share hydra_visualizer)/rviz/streaming_visualizer.rviz, + description: rviz file to load, + } + - node: { if: $(var start_rviz), name: rviz, pkg: rviz2, exec: rviz2, args: -d $(var rviz_path) } diff --git a/hydra_visualizer/rviz/lukas_trav.rviz b/hydra_visualizer/rviz/lukas_trav.rviz new file mode 100644 index 0000000..ecefa37 --- /dev/null +++ b/hydra_visualizer/rviz/lukas_trav.rviz @@ -0,0 +1,195 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Mesh1/Topic1 + - /Scene Graph1 + - /Trav_Layer1 + - /Trav_Layer1/Namespaces1 + - /Traversability Places1/Namespaces1 + Splitter Ratio: 0.5 + Tree Height: 1068 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: kimera_pgmo_rviz/MeshDisplay + Cull Backfaces: true + Enable Lighting: false + Enabled: false + Name: Mesh + Toggle Visibility All: true + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /hydra_visualizer/mesh + Value: false + Visible: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Scene Graph + Namespaces: + layer_2_bounding_boxes: false + layer_2_nodes: false + layer_2_text: false + layer_2p97_edges: true + layer_2p97_nodes: true + layer_2p97_text: false + layer_3p2_edges: true + layer_3p2_nodes: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /hydra_visualizer/graph + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Trav_Layer + Namespaces: + confidence: false + debug: true + state: false + traversability: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /hydra/traversability/layer + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traversability Places + Namespaces: + boundaries: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /hydra_visualizer/places_traversability + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5899993181228638 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 26.944629669189453 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0.5297571420669556 + Y: 23.26730728149414 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1359 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004b5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000780000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004b5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004b5000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009f40000003efc0100000002fb0000000800540069006d00650100000000000009f40000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000783000004b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2548 + X: -32 + Y: -32 diff --git a/hydra_visualizer/src/adapters/edge_color.cpp b/hydra_visualizer/src/adapters/edge_color.cpp index 631fcf7..98a51e0 100644 --- a/hydra_visualizer/src/adapters/edge_color.cpp +++ b/hydra_visualizer/src/adapters/edge_color.cpp @@ -42,6 +42,9 @@ #include "hydra_visualizer/color/color_parsing.h" +// TMP +#include + namespace hydra { using namespace spark_dsg; @@ -115,4 +118,53 @@ void declare_config(ValueEdgeColorAdapter::Config& config) { field(config.value_functor, "value_functor"); } +TraversabilityEdgeColorAdapter::TraversabilityEdgeColorAdapter(const Config& config) + : config(config), min_value_(0.0), max_value_(1.0), colormap_(config.colormap) {} + +void TraversabilityEdgeColorAdapter::setGraph(const DynamicSceneGraph& graph, + LayerId layer) { + if (!graph.hasLayer(layer)) { + return; + } + bool is_first = true; + for (const auto& [key, edge] : graph.getLayer(layer).edges()) { + const auto value = edge.attributes().weight; + if (value < 0.0) { + continue; + } + if (is_first) { + min_value_ = value; + max_value_ = value; + is_first = false; + } else { + min_value_ = std::min(value, min_value_); + max_value_ = std::max(value, max_value_); + } + } +} + +EdgeColor TraversabilityEdgeColorAdapter::getColor(const DynamicSceneGraph&, + const SceneGraphEdge& edge) const { + const double weight = edge.attributes().weight; + if (weight == -1.0) { + return {config.active_color, config.active_color}; + } + if (weight == -2.0) { + return {config.backend_color, config.backend_color}; + } + if (weight < 0.0) { + return {Color::gray(), Color::gray()}; + } + const auto color = colormap_.getColor(weight, min_value_, max_value_); + return {color, color}; +} + +void declare_config(TraversabilityEdgeColorAdapter::Config& config) { + using namespace config; + name("TraversabilityEdgeColorAdapter::Config"); + field(config.colormap, "colormap"); + field(config.active_color, "active_color"); + field(config.backend_color, "backend_color"); +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/traversability_plugin.cpp b/hydra_visualizer/src/plugins/traversability_plugin.cpp index 9cd5c52..4353b17 100644 --- a/hydra_visualizer/src/plugins/traversability_plugin.cpp +++ b/hydra_visualizer/src/plugins/traversability_plugin.cpp @@ -63,6 +63,7 @@ using spark_dsg::SceneGraphLayer; using spark_dsg::SceneGraphNode; using spark_dsg::TraversabilityNodeAttributes; using spark_dsg::TraversabilityState; +using spark_dsg::TravNodeAttributes; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -128,74 +129,96 @@ void TraversabilityPlugin::drawBoundaries(const Config& config, marker.ns = "boundaries"; marker.pose.orientation.w = 1.0; marker.scale.x = config.line_width; - - // Start and stop indices for the corner points of the boundary along the state - // direction. - static const std::array, 4> state_pairs = { - std::make_pair(1, 0), - std::make_pair(1, 2), - std::make_pair(2, 3), - std::make_pair(0, 3)}; - + marker.scale.y = config.line_width; + marker.scale.z = config.line_width; for (const auto& [node_id, node] : layer.nodes()) { - const auto& attrs = node->attributes(); - // Reset the marker. marker.id = id++; marker.points.clear(); marker.colors.clear(); - // Get the world frame positions of the boundary points, adjusted for the line width - // for non-overlapping rendering. bot-right, bot-left, top-left, top-right - std::vector pts; - pts.reserve(4); - pts.emplace_back(attrs.boundary.max.x() - config.line_width, - attrs.boundary.min.y() + config.line_width, - 0); - pts.emplace_back(attrs.boundary.min.x() + config.line_width, - attrs.boundary.min.y() + config.line_width, - 0); - pts.emplace_back(attrs.boundary.min.x() + config.line_width, - attrs.boundary.max.y() - config.line_width, - 0); - pts.emplace_back(attrs.boundary.max.x() - config.line_width, - attrs.boundary.max.y() - config.line_width, - 0); - for (auto& point : pts) { - point += attrs.position; - point.z() = config.slice_height; + auto block_attrs = node->tryAttributes(); + if (block_attrs) { + drawBlockBoundary(config, *block_attrs, marker); + } else { + auto region_attrs = node->tryAttributes(); + if (region_attrs) { + drawRegionBoundary(config, *region_attrs, marker); + } } - // Draw the boundary points as a line segment, where individual states break up the - // line in equal parts if present. - for (size_t i = 0; i < 4; ++i) { - const auto& states = attrs.boundary.states[i]; - const size_t start = state_pairs[i].first; - const size_t end = state_pairs[i].second; - tf2::convert(pts[start], marker.points.emplace_back()); // First point. - - if (states.empty()) { - // Single unknown boundary. - addBoundaryPoint(config, marker, pts[end], TraversabilityState::UNKNOWN, true); - continue; - } + tracker_.add(marker, msg); + } +} - // Add line segments for continuous states. - auto current_state = states[0]; - for (size_t j = 1; j < states.size(); ++j) { - if (states[j] != current_state) { - const double fraction = static_cast(j) / (states.size() - 1); - Eigen::Vector3d segment_point = - pts[start] * (1.0 - fraction) + pts[end] * fraction; - addBoundaryPoint(config, marker, segment_point, current_state); - current_state = states[j]; - } +void TraversabilityPlugin::drawBlockBoundary( + const Config& config, + const spark_dsg::TraversabilityNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const { + // Get the world frame positions of the boundary points, adjusted for the line width + // for non-overlapping rendering. bot-right, bot-left, top-left, top-right + std::vector pts; + pts.reserve(4); + pts.emplace_back(attrs.boundary.max.x() - config.line_width, + attrs.boundary.min.y() + config.line_width, + 0); + pts.emplace_back(attrs.boundary.min.x() + config.line_width, + attrs.boundary.min.y() + config.line_width, + 0); + pts.emplace_back(attrs.boundary.min.x() + config.line_width, + attrs.boundary.max.y() - config.line_width, + 0); + pts.emplace_back(attrs.boundary.max.x() - config.line_width, + attrs.boundary.max.y() - config.line_width, + 0); + for (auto& point : pts) { + point += attrs.position; + point.z() = config.slice_height; + } + + // Draw the boundary points as a line segment, where individual states break up the + // line in equal parts if present. + for (size_t i = 0; i < 4; ++i) { + const auto& states = attrs.boundary.states[i]; + const size_t start = state_pairs_[i].first; + const size_t end = state_pairs_[i].second; + tf2::convert(pts[start], marker.points.emplace_back()); // First point. + + if (states.empty()) { + // Single unknown boundary. + addBoundaryPoint(config, marker, pts[end], TraversabilityState::UNKNOWN, true); + continue; + } + + // Add line segments for continuous states. + auto current_state = states[0]; + for (size_t j = 1; j < states.size(); ++j) { + if (states[j] != current_state) { + const double fraction = static_cast(j) / (states.size() - 1); + Eigen::Vector3d segment_point = + pts[start] * (1.0 - fraction) + pts[end] * fraction; + addBoundaryPoint(config, marker, segment_point, current_state); + current_state = states[j]; } - addBoundaryPoint(config, marker, pts[end], current_state, true); } + addBoundaryPoint(config, marker, pts[end], current_state, true); + } +} - // Wrap around the last point to the first point. - tracker_.add(marker, msg); +void TraversabilityPlugin::drawRegionBoundary( + const Config& config, + const TravNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const { + marker.type = Marker::LINE_STRIP; + for (size_t i = 0; i < attrs.radii.size(); ++i) { + tf2::convert(attrs.getBoundaryPoint(i), marker.points.emplace_back()); + marker.colors.emplace_back( + visualizer::makeColorMsg(config.colors[static_cast(attrs.states[i])])); + } + // Close the circle. + if (!attrs.radii.empty()) { + marker.points.emplace_back(marker.points.front()); + marker.colors.emplace_back(marker.colors.front()); } }