Feature/rg traversability

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
@@ -47,10 +47,11 @@ renderer:
       visualize: true
       nodes: {scale: 0.2, color: {type: LabelColorAdapter}, 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

@@ -8,7 +8,6 @@ launch:
   - 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}
   # visualizer node and control for launching
   - node:
       pkg: hydra_visualizer
@@ -22,7 +21,6 @@ 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}

hydra_visualizer/launch/streaming_visualizer.launch.yaml

@@ -8,7 +8,6 @@ launch:
   - 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}
   # 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}
@@ -28,7 +27,6 @@ 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}

hydra_visualizer/src/adapters/edge_color.cpp

@@ -115,4 +115,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

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<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)};
-
+  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<TraversabilityNodeAttributes>();
-
     // 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<Eigen::Vector3d> 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<TraversabilityNodeAttributes>();
+    if (block_attrs) {
+      drawBlockBoundary(config, *block_attrs, marker);
+    } else {
+      auto region_attrs = node->tryAttributes<TravNodeAttributes>();
+      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<double>(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<Eigen::Vector3d> 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<double>(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<size_t>(attrs.states[i])]));
+  }
+  // Close the circle.
+  if (!attrs.radii.empty()) {
+    marker.points.emplace_back(marker.points.front());
+    marker.colors.emplace_back(marker.colors.front());
   }
 }