add specialized subscriber for labels as colors

hydra_ros/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 
 find_package(ament_cmake REQUIRED)
+find_package(CLI11 REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(gflags REQUIRED)
@@ -92,6 +93,9 @@ ament_target_dependencies(
 add_executable(hydra_ros_node app/hydra_node.cpp)
 target_link_libraries(hydra_ros_node ${PROJECT_NAME} ${gflags_LIBRARIES})
 
+add_executable(color_to_label_node app/color_to_label_node.cpp)
+target_link_libraries(color_to_label_node ${PROJECT_NAME} cv_bridge::cv_bridge CLI11::CLI11)
+
 rclcpp_components_register_node(
   ${PROJECT_NAME}
   PLUGIN
@@ -113,7 +117,7 @@ install(
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
-install(TARGETS hydra_ros_node RUNTIME DESTINATION lib/${PROJECT_NAME})
+install(TARGETS color_to_label_node hydra_ros_node RUNTIME DESTINATION lib/${PROJECT_NAME})
 install(
   PROGRAMS app/dsg_file_publisher
            app/dsg_republisher

hydra_ros/app/color_to_label_node.cpp

@@ -0,0 +1,142 @@
+/* -----------------------------------------------------------------------------
+ * 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/common/semantic_color_map.h>
+#include <ianvs/spin_functions.h>
+
+#include <filesystem>
+
+#include <CLI/CLI.hpp>
+#include <cv_bridge/cv_bridge.hpp>
+#include <cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+namespace hydra {
+
+using sensor_msgs::msg::Image;
+using ImageAdapter = rclcpp::TypeAdapter<cv_bridge::ROSCvMatContainer, Image>;
+
+struct ColorToLabelNode : public rclcpp::Node {
+  explicit ColorToLabelNode(const std::filesystem::path& colormap_path,
+                            size_t queue_size = 1);
+
+  void callback(const std_msgs::msg::Header& header, const cv::Mat& msg);
+
+  int32_t default_label;
+  std::unique_ptr<SemanticColorMap> colormap;
+  rclcpp::Publisher<Image>::SharedPtr pub;
+  rclcpp::Subscription<ImageAdapter>::SharedPtr sub;
+};
+
+ColorToLabelNode::ColorToLabelNode(const std::filesystem::path& colormap_path,
+                                   size_t queue_size)
+    : Node("color_to_label_node"),
+      default_label(-1),
+      colormap(SemanticColorMap::fromCsv(colormap_path)),
+      pub(create_publisher<Image>("labels", queue_size)) {
+  const auto cb = [this](const cv_bridge::ROSCvMatContainer& container) {
+    callback(container.header(), container.cv_mat());
+  };
+
+  sub = create_subscription<ImageAdapter>("colors", queue_size, cb);
+}
+
+void ColorToLabelNode::callback(const std_msgs::msg::Header& header,
+                                const cv::Mat& img) {
+  if (!colormap) {
+    RCLCPP_ERROR_STREAM(get_logger(), "Colormap is required!");
+    return;
+  }
+
+  if (img.empty() || img.channels() != 3) {
+    RCLCPP_ERROR_STREAM(get_logger(), "Failed to decode color image to semantics!");
+    return;
+  }
+
+  cv::Mat labels(img.size(), CV_32SC1);
+  for (int r = 0; r < img.rows; ++r) {
+    for (int c = 0; c < img.cols; ++c) {
+      const auto& pixel = img.at<cv::Vec3b>(r, c);
+      const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]);
+      labels.at<int32_t>(r, c) =
+          colormap->getLabelFromColor(color).value_or(default_label);
+    }
+  }
+
+  cv_bridge::CvImage to_pub(header, "32SC1", labels);
+  const auto msg = to_pub.toImageMsg();
+  pub->publish(*msg);
+}
+
+}  // namespace hydra
+
+struct AppArgs {
+  void add_to_app(CLI::App& app);
+
+  std::filesystem::path colormap_path;
+  int32_t default_label = -1;
+};
+
+void AppArgs::add_to_app(CLI::App& app) {
+  app.add_option("colormap_path", colormap_path)
+      ->required()
+      ->check(CLI::ExistingPath)
+      ->description("Colormap to use");
+  app.add_option("-l,--default-label", default_label);
+}
+
+int main(int argc, char* argv[]) {
+  CLI::App app("Utility node to convert labels stored by color to integer values");
+  argv = app.ensure_utf8(argv);
+  app.allow_extras();
+  app.get_formatter()->column_width(50);
+
+  AppArgs args;
+  args.add_to_app(app);
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  rclcpp::init(argc, argv);
+  {  // scope for node
+    hydra::ColorToLabelNode remapper(args.colormap_path);
+    ianvs::NodeHandle nh(remapper);
+    ianvs::spinAndWait(nh);
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

hydra_ros/include/hydra_ros/input/image_receiver.h

@@ -33,6 +33,7 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
+#include <hydra/common/semantic_color_map.h>
 #include <ianvs/node_handle.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
@@ -104,6 +105,25 @@ struct LabelSubscriber {
   std::shared_ptr<FilterSub<sensor_msgs::msg::Image>> impl_;
 };
 
+struct ColormappedLabelSubscriber {
+ public:
+  using MsgType = sensor_msgs::msg::Image;
+  using Filter = message_filters::SimpleFilter<MsgType>;
+
+  ColormappedLabelSubscriber();
+  explicit ColormappedLabelSubscriber(ianvs::NodeHandle nh, uint32_t queue_size = 1);
+  virtual ~ColormappedLabelSubscriber();
+
+  Filter& getFilter() const;
+  void setColormap(const SemanticColorMap* colormap, int32_t default_label);
+  void fillInput(const sensor_msgs::msg::Image& img, ImageInputPacket& packet) const;
+
+ private:
+  int32_t default_label_;
+  const SemanticColorMap* colormap_;
+  std::shared_ptr<FilterSub<sensor_msgs::msg::Image>> impl_;
+};
+
 struct FeatureSubscriber {
  public:
   using MsgType = semantic_inference_msgs::msg::FeatureImage;
@@ -213,6 +233,8 @@ class ClosedSetImageReceiver : public ImageReceiverImpl<LabelSubscriber> {
   virtual ~ClosedSetImageReceiver() = default;
 };
 
+void declare_config(ClosedSetImageReceiver::Config& config);
+
 class OpenSetImageReceiver : public ImageReceiverImpl<FeatureSubscriber> {
  public:
   struct Config : RosDataReceiver::Config {};
@@ -222,4 +244,24 @@ class OpenSetImageReceiver : public ImageReceiverImpl<FeatureSubscriber> {
 
 void declare_config(OpenSetImageReceiver::Config& config);
 
+class ColormappedLabelImageReceiver
+    : public ImageReceiverImpl<ColormappedLabelSubscriber> {
+ public:
+  struct Config : RosDataReceiver::Config {
+    //! Path to colormap CSV to use to remap colors to labels
+    std::filesystem::path colormap_path;
+    //! Label value to use when value is unknown
+    int32_t default_label = -1;
+  } const config;
+
+  ColormappedLabelImageReceiver(const Config& config, const std::string& sensor_name);
+  virtual ~ColormappedLabelImageReceiver() = default;
+  bool initImpl() override;
+
+ private:
+  std::unique_ptr<SemanticColorMap> colormap_;
+};
+
+void declare_config(ColormappedLabelImageReceiver::Config& config);
+
 }  // namespace hydra

hydra_ros/src/input/image_receiver.cpp

@@ -35,6 +35,8 @@
 #include "hydra_ros/input/image_receiver.h"
 
 #include <config_utilities/config.h>
+#include <config_utilities/types/path.h>
+#include <config_utilities/validation.h>
 #include <glog/logging.h>
 
 #include <cv_bridge/cv_bridge.hpp>
@@ -120,6 +122,58 @@ void LabelSubscriber::fillInput(const Image& img, ImageInputPacket& packet) cons
   }
 }
 
+ColormappedLabelSubscriber::ColormappedLabelSubscriber()
+    : default_label_(-1), colormap_(nullptr) {}
+
+ColormappedLabelSubscriber::ColormappedLabelSubscriber(ianvs::NodeHandle nh,
+                                                       uint32_t queue_size)
+    : default_label_(-1),
+      colormap_(nullptr),
+      impl_(std::make_shared<FilterSub<Image>>(nh, "semantic/image_raw", queue_size)) {}
+
+ColormappedLabelSubscriber::~ColormappedLabelSubscriber() = default;
+
+ColormappedLabelSubscriber::Filter& ColormappedLabelSubscriber::getFilter() const {
+  return *CHECK_NOTNULL(impl_);
+}
+
+void ColormappedLabelSubscriber::setColormap(const SemanticColorMap* colormap,
+                                             int32_t default_label) {
+  colormap_ = colormap;
+  default_label_ = default_label;
+}
+
+void ColormappedLabelSubscriber::fillInput(const Image& img,
+                                           ImageInputPacket& packet) const {
+  if (!colormap_) {
+    LOG(ERROR) << "Colormap not set for subscriber!";
+    return;
+  }
+
+  cv::Mat colors;
+  try {
+    colors = cv_bridge::toCvCopy(img)->image;
+  } catch (const cv_bridge::Exception& e) {
+    LOG(ERROR) << "Failed to convert label image: " << e.what();
+    return;
+  }
+
+  if (colors.empty() || colors.channels() != 3) {
+    LOG(ERROR) << "Failed to decode color image to semantics!";
+    return;
+  }
+
+  packet.labels = cv::Mat(colors.size(), CV_32SC1);
+  for (int r = 0; r < colors.rows; ++r) {
+    for (int c = 0; c < colors.cols; ++c) {
+      const auto& pixel = colors.at<cv::Vec3b>(r, c);
+      const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]);
+      packet.labels.at<int32_t>(r, c) =
+          colormap_->getLabelFromColor(color).value_or(default_label_);
+    }
+  }
+}
+
 FeatureSubscriber::FeatureSubscriber() = default;
 
 FeatureSubscriber::FeatureSubscriber(ianvs::NodeHandle nh, uint32_t queue_size)
@@ -148,12 +202,6 @@ 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) {}
@@ -180,26 +228,56 @@ void RGBDImageReceiver::callback(const sensor_msgs::msg::Image::ConstSharedPtr&
   queue.push(packet);
 }
 
-void declare_config(ClosedSetImageReceiver::Config& config) {
+void declare_config(RGBDImageReceiver::Config& config) {
   using namespace config;
-  name("ClosedSetImageReceiver::Config");
+  name("RGBDImageReceiver::Config");
   base<RosDataReceiver::Config>(config);
 }
 
 ClosedSetImageReceiver::ClosedSetImageReceiver(const Config& config,
                                                const std::string& sensor_name)
     : ImageReceiverImpl<LabelSubscriber>(config, sensor_name) {}
 
-void declare_config(OpenSetImageReceiver::Config& config) {
+void declare_config(ClosedSetImageReceiver::Config& config) {
   using namespace config;
-  name("OpenSetImageReceiver::Config");
-  base<hydra::RosDataReceiver::Config>(config);
+  name("ClosedSetImageReceiver::Config");
+  base<RosDataReceiver::Config>(config);
 }
 
 OpenSetImageReceiver::OpenSetImageReceiver(const Config& config,
                                            const std::string& sensor_name)
     : ImageReceiverImpl<FeatureSubscriber>(config, sensor_name) {}
 
+void declare_config(OpenSetImageReceiver::Config& config) {
+  using namespace config;
+  name("OpenSetImageReceiver::Config");
+  base<hydra::RosDataReceiver::Config>(config);
+}
+
+ColormappedLabelImageReceiver::ColormappedLabelImageReceiver(const Config& config,
+                                                             const std::string& name)
+    : ImageReceiverImpl<ColormappedLabelSubscriber>(config, name),
+      config(config::checkValid(config)),
+      colormap_(SemanticColorMap::fromCsv(config.colormap_path)) {
+  CHECK(colormap_) << "Colormap required!";
+}
+
+bool ColormappedLabelImageReceiver::initImpl() {
+  using Base = ImageReceiverImpl<ColormappedLabelSubscriber>;
+  const auto ret = Base::initImpl();
+  semantic_sub_.setColormap(colormap_.get(), config.default_label);
+  return ret;
+}
+
+void declare_config(ColormappedLabelImageReceiver::Config& config) {
+  using namespace config;
+  name("ColormappedLabelImageReceiver::Config");
+  base<hydra::RosDataReceiver::Config>(config);
+  field<Path::Absolute>(config.colormap_path, "colormap_path");
+  field(config.default_label, "default_label");
+  check<Path::Exists>(config.colormap_path, "colormap_path");
+}
+
 namespace {
 
 static const auto no_semantic_registration =
@@ -220,6 +298,11 @@ static const auto open_registration =
                                    OpenSetImageReceiver::Config,
                                    std::string>("OpenSetImageReceiver");
 
-}  // namespace
+static const auto color_registration =
+    config::RegistrationWithConfig<hydra::DataReceiver,
+                                   ColormappedLabelImageReceiver,
+                                   ColormappedLabelImageReceiver::Config,
+                                   std::string>("ColormappedLabelImageReceiver");
 
+}  // namespace
 }  // namespace hydra