Port/traversability (#35)

Schmluk · nathanhhughes · commit 7de8f3901041 · 2025-09-16T10:40:53.000-04:00
* port traversability visualization, builds

* fix segfaults

* run linting on all files

* address review comments
diff --git a/.gitignore b/.gitignore
@@ -214,3 +214,6 @@ pyrightconfig.json
 .vscode/
 
 .clangd
+
+# ROS2
+log/
diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt
@@ -41,6 +41,7 @@ add_library(
   src/frontend/object_visualizer.cpp
   src/frontend/places_visualizer.cpp
   src/frontend/ros_frontend_publisher.cpp
+  src/frontend/traversability_visualizer.cpp
   src/input/feature_receiver.cpp
   src/input/image_receiver.cpp
   src/input/pointcloud_adaptor.cpp
diff --git a/hydra_ros/include/hydra_ros/frontend/traversability_visualizer.h b/hydra_ros/include/hydra_ros/frontend/traversability_visualizer.h
@@ -0,0 +1,116 @@
+/* -----------------------------------------------------------------------------
+ * Copyright 2022 Massachusetts Institute of Technology.
+ * All Rights Reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above copyright notice,
+ *     this list of conditions and the following disclaimer in the documentation
+ *     and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Research was sponsored by the United States Air Force Research Laboratory and
+ * the United States Air Force Artificial Intelligence Accelerator and was
+ * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
+ * and conclusions contained in this document are those of the authors and should
+ * not be interpreted as representing the official policies, either expressed or
+ * implied, of the United States Air Force or the U.S. Government. The U.S.
+ * Government is authorized to reproduce and distribute reprints for Government
+ * purposes notwithstanding any copyright notation herein.
+ * -------------------------------------------------------------------------- */
+#pragma once
+
+#include <config_utilities/dynamic_config.h>
+#include <hydra/frontend/traversability_place_extractor.h>
+#include <hydra_visualizer/color/colormap_utilities.h>
+#include <ianvs/node_handle.h>
+
+#include <mutex>
+
+#include <visualization_msgs/msg/marker.hpp>
+
+namespace hydra {
+
+class TraversabilityVisualizer : public places::TraversabilityPlaceExtractor::Sink {
+ public:
+  struct Config {
+    //! Namespace to use for the visualizer node.
+    std::string ns = "~/traversability";
+    //! Colormap to use for traversability values.
+    visualizer::RangeColormap::Config traversability_colormap{
+        createSpectrumColormap({spark_dsg::Color::red(),
+                                spark_dsg::Color::yellow(),
+                                spark_dsg::Color::green()})};
+    //! Colormap to use for confidence values.
+    visualizer::RangeColormap::Config confidence_colormap{
+        createSpectrumColormap({spark_dsg::Color::black(),
+                                spark_dsg::Color::blue(),
+                                spark_dsg::Color::cyan()})};
+    //! Colors to use for the different traversability states (unknown, traversable,
+    //! intraversable, traversed).
+    std::vector<spark_dsg::Color> state_colors{spark_dsg::Color::black(),
+                                               spark_dsg::Color::blue(),
+                                               spark_dsg::Color::red(),
+                                               spark_dsg::Color::green()};
+    //! Height where the slice is visualized [m].
+    float drawing_offset_z = 0.0f;
+    //! True: Use height relative to the robot, false: use absolute height.
+    bool use_relative_offset = true;
+    //! Alpha value to use for the voxel colors.
+    double alpha = 0.3;
+  };
+
+  explicit TraversabilityVisualizer(const Config& config);
+
+  virtual ~TraversabilityVisualizer() = default;
+
+  std::string printInfo() const override;
+
+  void call(uint64_t timestamp_ns,
+            const Eigen::Vector3d& world_t_body,
+            const places::TraversabilityLayer& layer) const override;
+
+ protected:
+  config::DynamicConfig<Config> config_;
+  Config active_config_;  // Current config to use for a call.
+  mutable std::mutex config_mutex_;
+  ianvs::NodeHandle nh_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr layer_pub_;
+  std::unique_ptr<visualizer::RangeColormap> traversability_colormap_;
+  std::unique_ptr<visualizer::RangeColormap> confidence_colormap_;
+
+  void visualizeLayer(const std_msgs::msg::Header& header,
+                      const Eigen::Vector3d& world_t_body,
+                      const places::TraversabilityLayer& layer) const;
+
+  static visualizer::RangeColormap::Config createSpectrumColormap(
+      std::vector<spark_dsg::Color> colors);
+
+  Color debugColor(float value) const;
+
+  void onConfigUpdate();
+
+ private:
+  inline static const auto registration_ =
+      config::RegistrationWithConfig<places::TraversabilityPlaceExtractor::Sink,
+                                     TraversabilityVisualizer,
+                                     Config>("TraversabilityVisualizer");
+};
+
+void declare_config(TraversabilityVisualizer::Config& config);
+
+}  // namespace hydra
diff --git a/hydra_ros/src/frontend/places_visualizer.cpp b/hydra_ros/src/frontend/places_visualizer.cpp
@@ -44,6 +44,7 @@
 
 #include <rclcpp/time.hpp>
 
+#include "hydra_ros/frontend/gvd_visualization_utilities.h"
 #include "hydra_ros/visualizer/voxel_drawing.h"
 
 namespace hydra {
diff --git a/hydra_ros/src/frontend/traversability_visualizer.cpp b/hydra_ros/src/frontend/traversability_visualizer.cpp
@@ -0,0 +1,179 @@
+/* -----------------------------------------------------------------------------
+ * Copyright 2022 Massachusetts Institute of Technology.
+ * All Rights Reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above copyright notice,
+ *     this list of conditions and the following disclaimer in the documentation
+ *     and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Research was sponsored by the United States Air Force Research Laboratory and
+ * the United States Air Force Artificial Intelligence Accelerator and was
+ * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
+ * and conclusions contained in this document are those of the authors and should
+ * not be interpreted as representing the official policies, either expressed or
+ * implied, of the United States Air Force or the U.S. Government. The U.S.
+ * Government is authorized to reproduce and distribute reprints for Government
+ * purposes notwithstanding any copyright notation herein.
+ * -------------------------------------------------------------------------- */
+#include "hydra_ros/frontend/traversability_visualizer.h"
+
+#include <config_utilities/config.h>
+#include <config_utilities/printing.h>
+#include <config_utilities/validation.h>
+#include <hydra/common/global_info.h>
+#include <hydra_visualizer/color/color_parsing.h>
+#include <hydra_visualizer/color/colormap_utilities.h>
+#include <spark_dsg/colormaps.h>
+
+#include "hydra_ros/frontend/gvd_visualization_utilities.h"
+#include "hydra_ros/visualizer/voxel_drawing.h"
+
+namespace hydra {
+
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+using visualizer::ContinuousPalette;
+using visualizer::DivergentPalette;
+using visualizer::RangeColormap;
+
+void declare_config(TraversabilityVisualizer::Config& config) {
+  using namespace config;
+  name("TraversabilityVisualizer::Config");
+  field(config.ns, "ns");
+  field(config.traversability_colormap, "traversability_colormap");
+  field(config.confidence_colormap, "confidence_colormap");
+  field(config.state_colors, "state_colors");
+  field(config.drawing_offset_z, "drawing_offset_z", "m");
+  field(config.use_relative_offset, "use_relative_offset");
+  check(config.state_colors.size(), EQ, 4, "state_colors");
+}
+
+TraversabilityVisualizer::TraversabilityVisualizer(const Config& config)
+    : config_("traversability_visualizer",
+              config::checkValid(config),
+              [this]() { onConfigUpdate(); }),
+      nh_(ianvs::NodeHandle::this_node(config.ns)),
+      layer_pub_(nh_.create_publisher<visualization_msgs::msg::Marker>("layer", 10)) {
+  onConfigUpdate();
+}
+
+visualizer::RangeColormap::Config TraversabilityVisualizer::createSpectrumColormap(
+    std::vector<spark_dsg::Color> colors) {
+  visualizer::RangeColormap::Config config;
+  config.palette = visualizer::SpectrumPalette::Config{std::move(colors)};
+  return config;
+}
+
+std::string TraversabilityVisualizer::printInfo() const {
+  return config::toString(active_config_);
+}
+
+void TraversabilityVisualizer::onConfigUpdate() {
+  std::lock_guard<std::mutex> lock(config_mutex_);
+  active_config_ = config_.get();
+  traversability_colormap_ = std::make_unique<visualizer::RangeColormap>(
+      active_config_.traversability_colormap);
+  confidence_colormap_ =
+      std::make_unique<visualizer::RangeColormap>(active_config_.confidence_colormap);
+}
+
+void TraversabilityVisualizer::call(uint64_t timestamp_ns,
+                                    const Eigen::Vector3d& world_t_body,
+                                    const places::TraversabilityLayer& layer) const {
+  std_msgs::msg::Header header;
+  header.frame_id = GlobalInfo::instance().getFrames().map;
+  header.stamp = rclcpp::Time(timestamp_ns);
+  std::lock_guard<std::mutex> lock(config_mutex_);
+  visualizeLayer(header, world_t_body, layer);
+}
+
+void TraversabilityVisualizer::visualizeLayer(
+    const std_msgs::msg::Header& header,
+    const Eigen::Vector3d& world_t_body,
+    const places::TraversabilityLayer& layer) const {
+  visualization_msgs::msg::Marker msg;
+  msg.header = header;
+  msg.action = visualization_msgs::msg::Marker::ADD;
+  msg.id = 0;
+  msg.ns = "traversability";
+  msg.type = visualization_msgs::msg::Marker::CUBE_LIST;
+  msg.scale.x = layer.voxel_size;
+  msg.scale.y = layer.voxel_size;
+  msg.scale.z = layer.voxel_size;
+  msg.pose.orientation.w = 1.0;
+
+  visualization_msgs::msg::Marker msg2 = msg;
+  msg2.ns = "confidence";
+
+  visualization_msgs::msg::Marker msg3 = msg;
+  msg3.ns = "state";
+
+  visualization_msgs::msg::Marker msg4 = msg;
+  msg4.ns = "debug";
+
+  auto height = active_config_.drawing_offset_z;
+  if (active_config_.use_relative_offset) {
+    height += world_t_body.z();
+  }
+  geometry_msgs::msg::Point pos;
+  pos.z = height;
+
+  for (const auto& block : layer) {
+    for (size_t x = 0; x < block.voxels_per_side; ++x) {
+      pos.x = block.origin().x() + (x + 0.5f) * layer.voxel_size;
+      for (size_t y = 0; y < block.voxels_per_side; ++y) {
+        const auto& voxel = block.voxel(x, y);
+        if (voxel.confidence <= 0.0f) {
+          continue;  // Unobserved voxels.
+        }
+        pos.y = block.origin().y() + (y + 0.5f) * layer.voxel_size;
+        msg.points.push_back(pos);
+        msg.colors.push_back(visualizer::makeColorMsg(
+            traversability_colormap_->getColor(voxel.traversability),
+            active_config_.alpha));
+        msg2.colors.push_back(visualizer::makeColorMsg(
+            confidence_colormap_->getColor(voxel.confidence), active_config_.alpha));
+        msg3.colors.push_back(visualizer::makeColorMsg(
+            active_config_.state_colors[static_cast<uint8_t>(voxel.state)],
+            active_config_.alpha));
+        msg4.colors.push_back(visualizer::makeColorMsg(debugColor(voxel.debug_value),
+                                                       active_config_.alpha));
+      }
+    }
+  }
+  msg2.points = msg.points;
+  msg3.points = msg.points;
+  msg4.points = msg.points;
+
+  layer_pub_->publish(msg);
+  layer_pub_->publish(msg2);
+  layer_pub_->publish(msg3);
+  layer_pub_->publish(msg4);
+}
+
+Color TraversabilityVisualizer::debugColor(float value) const {
+  // Connected components.
+  if (value < 0.0f) {
+    return Color::black();
+  }
+  return spark_dsg::colormaps::rainbowId(static_cast<int>(value), 5);
+}
+
+}  // namespace hydra
diff --git a/hydra_visualizer/CMakeLists.txt b/hydra_visualizer/CMakeLists.txt
@@ -29,29 +29,30 @@ find_package(visualization_msgs REQUIRED)
 
 add_library(
   ${PROJECT_NAME}
-  src/drawing.cpp
-  src/layer_info.cpp
-  src/scene_graph_renderer.cpp
-  src/visualizer_node.cpp
+  src/plugins/traversability_plugin.cpp
   src/adapters/graph_color.cpp
   src/adapters/mesh_color.cpp
   src/adapters/text.cpp
   src/color/colormap_utilities.cpp
+  src/drawing.cpp
+  src/groundtruth/bounding_box_publisher.cpp
+  src/groundtruth/region_publisher.cpp
   src/io/graph_file_wrapper.cpp
   src/io/graph_ros_wrapper.cpp
   src/io/graph_zmq_wrapper.cpp
-  src/groundtruth/region_publisher.cpp
-  src/groundtruth/bounding_box_publisher.cpp
+  src/layer_info.cpp
   src/plugins/basis_point_plugin.cpp
-  src/plugins/places_freespace_plugin.cpp
+  src/plugins/footprint_plugin.cpp
   src/plugins/khronos_object_plugin.cpp
   src/plugins/mesh_plugin.cpp
-  src/plugins/footprint_plugin.cpp
+  src/plugins/places_freespace_plugin.cpp
   src/plugins/pose_plugin.cpp
+  src/scene_graph_renderer.cpp
   src/utils/ear_clipping.cpp
   src/utils/marker_group_pub.cpp
   src/utils/marker_tracker.cpp
   src/utils/polygon_utilities.cpp
+  src/visualizer_node.cpp
 )
 target_include_directories(
   ${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
diff --git a/hydra_visualizer/config/visualizer_plugins.yaml b/hydra_visualizer/config/visualizer_plugins.yaml
@@ -13,6 +13,8 @@ plugins:
   khronos_objects:
     type: KhronosObjectPlugin
     dynamic_color_mode: CONSTANT # ID, SEMANTIC, CONSTANT
+  places_traversability:
+    type: TraversabilityPlugin
   agent_poses:
     type: PosePlugin
     num_to_skip: 0
diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/text.h b/hydra_visualizer/include/hydra_visualizer/adapters/text.h
@@ -99,4 +99,14 @@ struct NameIdTextAdapter : GraphTextAdapter {
 
 void declare_config(NameIdTextAdapter::Config& config);
 
+struct AttributesTextAdaptor : GraphTextAdapter {
+  struct Config {};
+  explicit AttributesTextAdaptor(const Config&) {}
+  virtual ~AttributesTextAdaptor() = default;
+  std::string getText(const spark_dsg::DynamicSceneGraph& graph,
+                      const spark_dsg::SceneGraphNode& node) const override;
+};
+
+void declare_config(AttributesTextAdaptor::Config& config);
+
 }  // namespace hydra::visualizer
diff --git a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h
@@ -166,8 +166,9 @@ struct RangeColormap {
   } const config;
 
   explicit RangeColormap(const Config& config);
-  spark_dsg::Color getColor(double value, double min, double max) const;
-  spark_dsg::Color operator()(double value, double min, double max) const {
+
+  spark_dsg::Color getColor(double value, double min = 0.0, double max = 1.0) const;
+  spark_dsg::Color operator()(double value, double min = 0.0, double max = 1.0) const {
     return getColor(value, min, max);
   }
 
diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h
diff --git a/hydra_visualizer/src/adapters/text.cpp b/hydra_visualizer/src/adapters/text.cpp
diff --git a/hydra_visualizer/src/drawing.cpp b/hydra_visualizer/src/drawing.cpp
diff --git a/hydra_visualizer/src/plugins/traversability_plugin.cpp b/hydra_visualizer/src/plugins/traversability_plugin.cpp

-Original file line number
+Diff line change
 .vscode/
 .clangd
++
 +# ROS2
 +log/