From ee918d6753070e678878bf100fc85a3322fee030 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Wed, 1 Jul 2026 20:59:08 +0000 Subject: [PATCH 1/3] add specialized subscriber for labels as colors --- .../include/hydra_ros/input/image_receiver.h | 38 +++++++ hydra_ros/src/input/image_receiver.cpp | 103 ++++++++++++++++-- 2 files changed, 129 insertions(+), 12 deletions(-) diff --git a/hydra_ros/include/hydra_ros/input/image_receiver.h b/hydra_ros/include/hydra_ros/input/image_receiver.h index 04a224ed..68a2c47a 100644 --- a/hydra_ros/include/hydra_ros/input/image_receiver.h +++ b/hydra_ros/include/hydra_ros/input/image_receiver.h @@ -33,6 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once +#include #include #include #include @@ -104,6 +105,24 @@ struct LabelSubscriber { std::shared_ptr> impl_; }; +struct ColormappedLabelSubscriber { + public: + using MsgType = sensor_msgs::msg::Image; + using Filter = message_filters::SimpleFilter; + + ColormappedLabelSubscriber(); + explicit ColormappedLabelSubscriber(ianvs::NodeHandle nh, uint32_t queue_size = 1); + virtual ~ColormappedLabelSubscriber(); + + Filter& getFilter() const; + void setColormap(const SemanticColorMap* colormap); + void fillInput(const sensor_msgs::msg::Image& img, ImageInputPacket& packet) const; + + private: + const SemanticColorMap* colormap_; + std::shared_ptr> impl_; +}; + struct FeatureSubscriber { public: using MsgType = semantic_inference_msgs::msg::FeatureImage; @@ -213,6 +232,8 @@ class ClosedSetImageReceiver : public ImageReceiverImpl { virtual ~ClosedSetImageReceiver() = default; }; +void declare_config(ClosedSetImageReceiver::Config& config); + class OpenSetImageReceiver : public ImageReceiverImpl { public: struct Config : RosDataReceiver::Config {}; @@ -222,4 +243,21 @@ class OpenSetImageReceiver : public ImageReceiverImpl { void declare_config(OpenSetImageReceiver::Config& config); +class ColormappedLabelImageReceiver + : public ImageReceiverImpl { + public: + struct Config : RosDataReceiver::Config { + std::filesystem::path colormap_path; + } const config; + + ColormappedLabelImageReceiver(const Config& config, const std::string& sensor_name); + virtual ~ColormappedLabelImageReceiver() = default; + bool initImpl() override; + + private: + std::unique_ptr colormap_; +}; + +void declare_config(ColormappedLabelImageReceiver::Config& config); + } // namespace hydra diff --git a/hydra_ros/src/input/image_receiver.cpp b/hydra_ros/src/input/image_receiver.cpp index ef40cdbb..8d111ae9 100644 --- a/hydra_ros/src/input/image_receiver.cpp +++ b/hydra_ros/src/input/image_receiver.cpp @@ -35,6 +35,8 @@ #include "hydra_ros/input/image_receiver.h" #include +#include +#include #include #include @@ -120,6 +122,55 @@ void LabelSubscriber::fillInput(const Image& img, ImageInputPacket& packet) cons } } +ColormappedLabelSubscriber::ColormappedLabelSubscriber() : colormap_(nullptr) {} + +ColormappedLabelSubscriber::ColormappedLabelSubscriber(ianvs::NodeHandle nh, + uint32_t queue_size) + : colormap_(nullptr), + impl_(std::make_shared>(nh, "semantic/image_raw", queue_size)) {} + +ColormappedLabelSubscriber::~ColormappedLabelSubscriber() = default; + +ColormappedLabelSubscriber::Filter& ColormappedLabelSubscriber::getFilter() const { + return *CHECK_NOTNULL(impl_); +} + +void ColormappedLabelSubscriber::setColormap(const SemanticColorMap* colormap) { + colormap_ = colormap; +} + +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(r, c); + const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]); + // this is lazy, but works out to the same invalid label we normally use + packet.labels.at(r, c) = + colormap_->getLabelFromColor(color).value_or(-1); + } + } +} + FeatureSubscriber::FeatureSubscriber() = default; FeatureSubscriber::FeatureSubscriber(ianvs::NodeHandle nh, uint32_t queue_size) @@ -148,12 +199,6 @@ void FeatureSubscriber::fillInput(const MsgType& msg, ImageInputPacket& packet) } } -void declare_config(RGBDImageReceiver::Config& config) { - using namespace config; - name("RGBDImageReceiver::Config"); - base(config); -} - RGBDImageReceiver::RGBDImageReceiver(const Config& config, const std::string& sensor_name) : RosDataReceiver(config, sensor_name) {} @@ -180,9 +225,9 @@ 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(config); } @@ -190,16 +235,45 @@ ClosedSetImageReceiver::ClosedSetImageReceiver(const Config& config, const std::string& sensor_name) : ImageReceiverImpl(config, sensor_name) {} -void declare_config(OpenSetImageReceiver::Config& config) { +void declare_config(ClosedSetImageReceiver::Config& config) { using namespace config; - name("OpenSetImageReceiver::Config"); - base(config); + name("ClosedSetImageReceiver::Config"); + base(config); } OpenSetImageReceiver::OpenSetImageReceiver(const Config& config, const std::string& sensor_name) : ImageReceiverImpl(config, sensor_name) {} +void declare_config(OpenSetImageReceiver::Config& config) { + using namespace config; + name("OpenSetImageReceiver::Config"); + base(config); +} + +ColormappedLabelImageReceiver::ColormappedLabelImageReceiver(const Config& config, + const std::string& name) + : ImageReceiverImpl(config, name), + config(config::checkValid(config)), + colormap_(SemanticColorMap::fromCsv(config.colormap_path)) { + CHECK(colormap_) << "Colormap required!"; +} + +bool ColormappedLabelImageReceiver::initImpl() { + using Base = ImageReceiverImpl; + const auto ret = Base::initImpl(); + semantic_sub_.setColormap(colormap_.get()); + return ret; +} + +void declare_config(ColormappedLabelImageReceiver::Config& config) { + using namespace config; + name("ColormappedLabelImageReceiver::Config"); + base(config); + field(config.colormap_path, "colormap_path"); + check(config.colormap_path, "colormap_path"); +} + namespace { static const auto no_semantic_registration = @@ -220,6 +294,11 @@ static const auto open_registration = OpenSetImageReceiver::Config, std::string>("OpenSetImageReceiver"); -} // namespace +static const auto color_registration = + config::RegistrationWithConfig("ColormappedLabelImageReceiver"); +} // namespace } // namespace hydra From 0db3008853984584899a4a35b4047fbca58a9bc9 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Wed, 1 Jul 2026 21:04:28 +0000 Subject: [PATCH 2/3] make default label configurable --- .../include/hydra_ros/input/image_receiver.h | 6 +++++- hydra_ros/src/input/image_receiver.cpp | 16 ++++++++++------ 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/hydra_ros/include/hydra_ros/input/image_receiver.h b/hydra_ros/include/hydra_ros/input/image_receiver.h index 68a2c47a..f60f5a36 100644 --- a/hydra_ros/include/hydra_ros/input/image_receiver.h +++ b/hydra_ros/include/hydra_ros/input/image_receiver.h @@ -115,10 +115,11 @@ struct ColormappedLabelSubscriber { virtual ~ColormappedLabelSubscriber(); Filter& getFilter() const; - void setColormap(const SemanticColorMap* colormap); + 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> impl_; }; @@ -247,7 +248,10 @@ class ColormappedLabelImageReceiver : public ImageReceiverImpl { 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); diff --git a/hydra_ros/src/input/image_receiver.cpp b/hydra_ros/src/input/image_receiver.cpp index 8d111ae9..61044360 100644 --- a/hydra_ros/src/input/image_receiver.cpp +++ b/hydra_ros/src/input/image_receiver.cpp @@ -122,11 +122,13 @@ void LabelSubscriber::fillInput(const Image& img, ImageInputPacket& packet) cons } } -ColormappedLabelSubscriber::ColormappedLabelSubscriber() : colormap_(nullptr) {} +ColormappedLabelSubscriber::ColormappedLabelSubscriber() + : default_label_(-1), colormap_(nullptr) {} ColormappedLabelSubscriber::ColormappedLabelSubscriber(ianvs::NodeHandle nh, uint32_t queue_size) - : colormap_(nullptr), + : default_label_(-1), + colormap_(nullptr), impl_(std::make_shared>(nh, "semantic/image_raw", queue_size)) {} ColormappedLabelSubscriber::~ColormappedLabelSubscriber() = default; @@ -135,8 +137,10 @@ ColormappedLabelSubscriber::Filter& ColormappedLabelSubscriber::getFilter() cons return *CHECK_NOTNULL(impl_); } -void ColormappedLabelSubscriber::setColormap(const SemanticColorMap* colormap) { +void ColormappedLabelSubscriber::setColormap(const SemanticColorMap* colormap, + int32_t default_label) { colormap_ = colormap; + default_label_ = default_label; } void ColormappedLabelSubscriber::fillInput(const Image& img, @@ -164,9 +168,8 @@ void ColormappedLabelSubscriber::fillInput(const Image& img, for (int c = 0; c < colors.cols; ++c) { const auto& pixel = colors.at(r, c); const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]); - // this is lazy, but works out to the same invalid label we normally use packet.labels.at(r, c) = - colormap_->getLabelFromColor(color).value_or(-1); + colormap_->getLabelFromColor(color).value_or(default_label_); } } } @@ -262,7 +265,7 @@ ColormappedLabelImageReceiver::ColormappedLabelImageReceiver(const Config& confi bool ColormappedLabelImageReceiver::initImpl() { using Base = ImageReceiverImpl; const auto ret = Base::initImpl(); - semantic_sub_.setColormap(colormap_.get()); + semantic_sub_.setColormap(colormap_.get(), config.default_label); return ret; } @@ -271,6 +274,7 @@ void declare_config(ColormappedLabelImageReceiver::Config& config) { name("ColormappedLabelImageReceiver::Config"); base(config); field(config.colormap_path, "colormap_path"); + field(config.default_label, "default_label"); check(config.colormap_path, "colormap_path"); } From a33aea02b695130f17cc2cb4e5e31a464891593b Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Thu, 2 Jul 2026 00:01:05 +0000 Subject: [PATCH 3/3] also add color conversion utility node --- hydra_ros/CMakeLists.txt | 6 +- hydra_ros/app/color_to_label_node.cpp | 142 ++++++++++++++++++++++++++ 2 files changed, 147 insertions(+), 1 deletion(-) create mode 100644 hydra_ros/app/color_to_label_node.cpp diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index e1018ee4..d222fc21 100644 --- a/hydra_ros/CMakeLists.txt +++ b/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 diff --git a/hydra_ros/app/color_to_label_node.cpp b/hydra_ros/app/color_to_label_node.cpp new file mode 100644 index 00000000..a1d55be3 --- /dev/null +++ b/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 +#include + +#include + +#include +#include +#include +#include + +namespace hydra { + +using sensor_msgs::msg::Image; +using ImageAdapter = rclcpp::TypeAdapter; + +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 colormap; + rclcpp::Publisher::SharedPtr pub; + rclcpp::Subscription::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("labels", queue_size)) { + const auto cb = [this](const cv_bridge::ROSCvMatContainer& container) { + callback(container.header(), container.cv_mat()); + }; + + sub = create_subscription("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(r, c); + const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]); + labels.at(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; +}