Feature/rg traversability

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

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<visualization_msgs::msg::Marker>("layer", 10)) {
+      layer_pub_(nh_.create_publisher<visualization_msgs::msg::Marker>("layer", 100)) {
   onConfigUpdate();
 }
 

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

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::ContinuousPalette>{
+            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

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> config_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::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<std::pair<size_t, size_t>, 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);

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 }

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) }