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; +} diff --git a/hydra_ros/include/hydra_ros/input/image_receiver.h b/hydra_ros/include/hydra_ros/input/image_receiver.h index 04a224ed..f60f5a36 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,25 @@ 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, 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_; +}; + struct FeatureSubscriber { public: using MsgType = semantic_inference_msgs::msg::FeatureImage; @@ -213,6 +233,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 +244,24 @@ class OpenSetImageReceiver : public ImageReceiverImpl { void declare_config(OpenSetImageReceiver::Config& config); +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); + 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..61044360 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,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>(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(r, c); + const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]); + packet.labels.at(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(config); -} - RGBDImageReceiver::RGBDImageReceiver(const Config& config, const std::string& sensor_name) : RosDataReceiver(config, sensor_name) {} @@ -180,9 +228,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 +238,46 @@ 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(), config.default_label); + return ret; +} + +void declare_config(ColormappedLabelImageReceiver::Config& config) { + using namespace config; + name("ColormappedLabelImageReceiver::Config"); + base(config); + field(config.colormap_path, "colormap_path"); + field(config.default_label, "default_label"); + check(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("ColormappedLabelImageReceiver"); +} // namespace } // namespace hydra