Feature/geometry only

hydra_ros/CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(
   src/input/ros_data_receiver.cpp
   src/input/ros_input_module.cpp
   src/input/ros_sensors.cpp
+  src/places/external_traversability_estimator.cpp
   src/odometry/ros_pose_graph_tracker.cpp
   src/openset/ros_embedding_group.cpp
   src/utils/bow_subscriber.cpp

hydra_ros/include/hydra_ros/input/image_receiver.h

@@ -182,6 +182,30 @@ void ImageReceiverImpl<SemanticT>::callback(
   queue.push(packet);
 }
 
+class RGBDImageReceiver : public RosDataReceiver {
+ public:
+  struct Config : RosDataReceiver::Config {};
+  using Policy =
+      message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,
+                                                      sensor_msgs::msg::Image>;
+  using Synchronizer = message_filters::Synchronizer<Policy>;
+
+  RGBDImageReceiver(const Config& config, const std::string& sensor_name);
+  virtual ~RGBDImageReceiver() = default;
+
+ protected:
+  bool initImpl() override;
+
+  void callback(const sensor_msgs::msg::Image::ConstSharedPtr& color,
+                const sensor_msgs::msg::Image::ConstSharedPtr& depth);
+
+  ColorSubscriber color_sub_;
+  DepthSubscriber depth_sub_;
+  std::unique_ptr<Synchronizer> sync_;
+};
+
+void declare_config(RGBDImageReceiver::Config& config);
+
 class ClosedSetImageReceiver : public ImageReceiverImpl<LabelSubscriber> {
  public:
   struct Config : RosDataReceiver::Config {};

hydra_ros/include/hydra_ros/places/external_traversability_estimator.h

@@ -0,0 +1,81 @@
+/* -----------------------------------------------------------------------------
+ * 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 <hydra/places/traversability_estimator.h>
+
+#include <mutex>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace hydra::places {
+
+/**
+ * @brief Takes in an external traversability map as occupancy grid to perform
+ * traversability analysis.
+ */
+class ExternalTraversabilityEstimator : public TraversabilityEstimator {
+ public:
+  struct Config {
+    //! @brief The height above the robot body to consider for traversability in meters.
+    std::string input_topic = "/local_cost_map";
+    unsigned int queue_size = 2;
+  } const config;
+
+  using State = spark_dsg::TraversabilityState;
+
+  ExternalTraversabilityEstimator(const Config& config);
+  ~ExternalTraversabilityEstimator() override = default;
+
+  void updateTraversability(const ActiveWindowOutput& msg,
+                            const kimera_pgmo::MeshDelta& /* mesh_delta */,
+                            const spark_dsg::DynamicSceneGraph& /* graph */) override;
+
+  void callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+
+  const TraversabilityLayer& getTraversabilityLayer() const override;
+
+ protected:
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_;
+  mutable std::mutex mutex_;
+
+ protected:
+  State occupancyToTraversability(int8_t occupancy) const;
+};
+
+void declare_config(ExternalTraversabilityEstimator::Config& config);
+
+}  // namespace hydra::places

hydra_ros/src/frontend/traversability_visualizer.cpp

@@ -173,7 +173,7 @@ Color TraversabilityVisualizer::debugColor(float value) const {
   if (value < 0.0f) {
     return Color::black();
   }
-  return spark_dsg::colormaps::rainbowId(static_cast<int>(value), 5);
+  return spark_dsg::colormaps::rainbowId(static_cast<int>(value), 10);
 }
 
 }  // namespace hydra

hydra_ros/src/input/image_receiver.cpp

@@ -56,11 +56,28 @@ ColorSubscriber::Filter& ColorSubscriber::getFilter() const {
 }
 
 void ColorSubscriber::fillInput(const Image& img, ImageInputPacket& packet) const {
-  try {
-    packet.color = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image;
-  } catch (const cv_bridge::Exception& e) {
-    LOG(ERROR) << "Failed to convert color image: " << e.what();
+  // Allow also mono images to be converted to grayscale.
+  if (sensor_msgs::image_encodings::isColor(img.encoding)) {
+    try {
+      packet.color =
+          cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image;
+      return;
+    } catch (const cv_bridge::Exception& e) {
+      LOG(ERROR) << "Failed to convert mono image as color input: " << e.what();
+      return;
+    }
+  } else if (sensor_msgs::image_encodings::isMono(img.encoding)) {
+    try {
+      cv::Mat mono =
+          cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8)->image;
+      cv::cvtColor(mono, packet.color, cv::COLOR_GRAY2RGB);
+      return;
+    } catch (const cv_bridge::Exception& e) {
+      LOG(ERROR) << "Failed to convert color image: " << e.what();
+      return;
+    }
   }
+  LOG(ERROR) << "Failed to convert color image: unsupported encoding: " << img.encoding;
 }
 
 DepthSubscriber::DepthSubscriber() = default;
@@ -130,6 +147,38 @@ void FeatureSubscriber::fillInput(const MsgType& msg, ImageInputPacket& packet)
   }
 }
 
+void declare_config(RGBDImageReceiver::Config& config) {
+  using namespace config;
+  name("RGBDImageReceiver::Config");
+  base<RosDataReceiver::Config>(config);
+}
+
+RGBDImageReceiver::RGBDImageReceiver(const Config& config,
+                                     const std::string& sensor_name)
+    : RosDataReceiver(config, sensor_name) {}
+
+bool RGBDImageReceiver::initImpl() {
+  color_sub_ = ColorSubscriber(ianvs::NodeHandle::this_node(ns_));
+  depth_sub_ = DepthSubscriber(ianvs::NodeHandle::this_node(ns_));
+  sync_.reset(new Synchronizer(
+      Policy(config.queue_size), color_sub_.getFilter(), depth_sub_.getFilter()));
+  sync_->registerCallback(&RGBDImageReceiver::callback, this);
+  return true;
+}
+
+void RGBDImageReceiver::callback(const sensor_msgs::msg::Image::ConstSharedPtr& color,
+                                 const sensor_msgs::msg::Image::ConstSharedPtr& depth) {
+  const auto timestamp_ns = rclcpp::Time(color->header.stamp).nanoseconds();
+  if (!checkInputTimestamp(timestamp_ns)) {
+    return;
+  }
+
+  auto packet = std::make_shared<ImageInputPacket>(timestamp_ns, sensor_name_);
+  color_sub_.fillInput(*color, *packet);
+  depth_sub_.fillInput(*depth, *packet);
+  queue.push(packet);
+}
+
 void declare_config(ClosedSetImageReceiver::Config& config) {
   using namespace config;
   name("ClosedSetImageReceiver::Config");
@@ -152,6 +201,12 @@ OpenSetImageReceiver::OpenSetImageReceiver(const Config& config,
 
 namespace {
 
+static const auto no_semantic_registration =
+    config::RegistrationWithConfig<DataReceiver,
+                                   RGBDImageReceiver,
+                                   RGBDImageReceiver::Config,
+                                   std::string>("RGBDImageReceiver");
+
 static const auto closed_registration =
     config::RegistrationWithConfig<DataReceiver,
                                    ClosedSetImageReceiver,

hydra_ros/src/places/external_traversability_estimator.cpp

@@ -0,0 +1,151 @@
+/* -----------------------------------------------------------------------------
+ * 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/places/external_traversability_estimator.h"
+
+#include <config_utilities/config.h>
+#include <config_utilities/validation.h>
+#include <ianvs/node_handle.h>
+#include <spark_dsg/traversability_boundary.h>
+
+namespace hydra::places {
+
+using spark_dsg::TraversabilityState;
+
+static const auto registration =
+    config::RegistrationWithConfig<TraversabilityEstimator,
+                                   ExternalTraversabilityEstimator,
+                                   ExternalTraversabilityEstimator::Config>(
+        "ExternalTraversabilityEstimator");
+
+void declare_config(ExternalTraversabilityEstimator::Config& config) {
+  using namespace config;
+  name("ExternalTraversabilityEstimator::Config");
+  field(config.input_topic, "input_topic");
+  field(config.queue_size, "queue_size");
+  checkCondition(!config.input_topic.empty(), "'input_topic' must be specified");
+}
+
+ExternalTraversabilityEstimator::ExternalTraversabilityEstimator(const Config& config)
+    : config(config::checkValid(config)) {
+  auto nh = ianvs::NodeHandle::this_node();
+  sub_ = nh.create_subscription<nav_msgs::msg::OccupancyGrid>(
+      config.input_topic,
+      config.queue_size,
+      &ExternalTraversabilityEstimator::callback,
+      this);
+}
+
+const TraversabilityLayer& ExternalTraversabilityEstimator::getTraversabilityLayer()
+    const {
+  std::lock_guard<std::mutex> lock(mutex_);
+  return *traversability_layer_;
+}
+
+void ExternalTraversabilityEstimator::updateTraversability(
+    const ActiveWindowOutput& msg,
+    const kimera_pgmo::MeshDelta&,
+    const spark_dsg::DynamicSceneGraph&) {
+  if (!traversability_layer_) {
+    const auto& map_config = msg.map().config;
+    traversability_layer_ = std::make_unique<TraversabilityLayer>(
+        map_config.voxel_size, map_config.voxels_per_side);
+  }
+}
+
+void ExternalTraversabilityEstimator::callback(
+    const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+  if (!traversability_layer_) {
+    return;
+  }
+  // Reset update tracking.
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (auto& block : *traversability_layer_) {
+    block.updated = false;
+    for (auto& voxel : block.voxels) {
+      voxel.confidence = 0.0f;
+    }
+  }
+
+  // Updated the stored layer with the map.
+  const float voxel_size = msg->info.resolution;
+  const Eigen::Vector3f origin(
+      msg->info.origin.position.x, msg->info.origin.position.y, 0.0f);
+  for (unsigned int y = 0; y < msg->info.height; ++y) {
+    for (unsigned int x = 0; x < msg->info.width; ++x) {
+      const int idx = x + y * msg->info.width;
+      const State state = occupancyToTraversability(msg->data[idx]);
+      // Compute the voxel coordinates.
+      const Eigen::Vector3f position =
+          Eigen::Vector3f(x, y, 0.0f) * voxel_size + origin;
+      const auto index = traversability_layer_->globalIndexFromPoint(position);
+      // TODO(lschmid): Fix the allocate block interfaces for the traversability layer.
+      auto& block = traversability_layer_->allocateBlock(
+          traversability_layer_->blockIndexFromGlobal(index),
+          traversability_layer_->voxels_per_side);
+      block.updated = true;
+      auto& voxel = block.voxel(traversability_layer_->voxelIndexFromGlobal(index));
+      if (voxel.confidence == 0.0f) {
+        // Overwrite if this is the first observation this iteration.
+        voxel.state = state;
+        voxel.confidence = 1.0f;
+      } else {
+        spark_dsg::fuseStates(state, voxel.state);
+        voxel.confidence += 1.0f;
+      }
+    }
+  }
+
+  // Remove blocks that were not updated.
+  spatial_hash::BlockIndices to_remove;
+  for (auto& block : *traversability_layer_) {
+    if (!block.updated) {
+      to_remove.push_back(block.index);
+    }
+  }
+  traversability_layer_->removeBlocks(to_remove);
+}
+
+ExternalTraversabilityEstimator::State
+ExternalTraversabilityEstimator::occupancyToTraversability(int8_t occupancy) const {
+  if (occupancy < 0) {
+    return State::UNKNOWN;
+  }
+  if (occupancy == 0) {
+    return State::TRAVERSABLE;
+  }
+  return State::INTRAVERSABLE;
+}
+
+}  // namespace hydra::places