From 98dbee9ba2eda6cf73be4d4296baa7bc314eb9b4 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Tue, 30 Jun 2026 17:16:10 +0000 Subject: [PATCH 1/2] first pass at receiving labelspace from semantic inference --- hydra_ros/CMakeLists.txt | 1 + .../include/hydra_ros/input/ros_labelspace.h | 58 ++++++++ hydra_ros/src/input/ros_labelspace.cpp | 126 ++++++++++++++++++ 3 files changed, 185 insertions(+) create mode 100644 hydra_ros/include/hydra_ros/input/ros_labelspace.h create mode 100644 hydra_ros/src/input/ros_labelspace.cpp diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index d222fc21..4a43e299 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -50,6 +50,7 @@ add_library( src/input/pointcloud_adaptor.cpp src/input/pointcloud_receiver.cpp src/input/ros_data_receiver.cpp + src/input/ros_labelspace.cpp src/input/ros_input_module.cpp src/input/ros_sensors.cpp src/places/external_traversability_estimator.cpp diff --git a/hydra_ros/include/hydra_ros/input/ros_labelspace.h b/hydra_ros/include/hydra_ros/input/ros_labelspace.h new file mode 100644 index 00000000..643d7206 --- /dev/null +++ b/hydra_ros/include/hydra_ros/input/ros_labelspace.h @@ -0,0 +1,58 @@ +/* ----------------------------------------------------------------------------- + * 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 + +namespace hydra { + +class RosLabelspace : public Labelspace { + public: + struct Config { + //! @brief Optional namespace to use when listening for labelspace message + std::string ns = ""; + //! @brief Amount of time before warning about missing labelspace (0 disables) + double warning_timeout_s = 10.0; + //! @brief Amount of time to wait before forcing Hydra to exit (0 disables) + double error_timeout_s = 0.0; + //! @brief Whether or not to use latching for subscriber + bool latch_info_sub = false; + } const config; + + explicit RosLabelspace(const Config& config); +}; + +void declare_config(RosLabelspace::Config& config); + +} // namespace hydra diff --git a/hydra_ros/src/input/ros_labelspace.cpp b/hydra_ros/src/input/ros_labelspace.cpp new file mode 100644 index 00000000..20c81f14 --- /dev/null +++ b/hydra_ros/src/input/ros_labelspace.cpp @@ -0,0 +1,126 @@ +/* ----------------------------------------------------------------------------- + * 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/common/ros_labelspace.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace hydra { +namespace { + +static const auto reg = + config::RegistrationWithConfig( + "from_msg"); + +} + +using nlohmann::json; +using MsgType = semantic_inference_msgs::msg::Labelspace; + +RosLabelspace::RosLabelspace(const Config& config) : config(config) { + auto nh = ianvs::NodeHandle::this_node(config.ns); + const auto resolved_topic = nh.resolve_name("labelspace", false); + LOG(INFO) << "Waiting for labelspace on '" << resolved_topic << "'"; + + std::optional maybe_msg; + const auto start = nh.now(); + const auto qos = rclcpp::QoS(1).durability( + config.latch_info_sub ? rclcpp::DurabilityPolicy::TransientLocal + : rclcpp::DurabilityPolicy::Volatile); + const size_t timeout = std::floor(config.warning_timeout_s * 1000); + while (!maybe_msg && rclcpp::ok()) { + maybe_msg = ianvs::getSingleMessage(nh, "labelspace", true, qos, timeout); + if (!maybe_msg) { + LOG(WARNING) << "Waiting for CameraInfo on topic '" << resolved_topic << "'"; + } + + const auto diff = nh.now() - start; + if (config.error_timeout_s && (diff.seconds() > config.error_timeout_s)) { + LOG(ERROR) << "Sensor intrinsics lookup timed out on '" << resolved_topic << "'"; + break; + } + } + + if (rclcpp::ok() && !maybe_msg) { + LOG(FATAL) << "Failed to lookup labelspace on '" << resolved_topic << "'"; + } + + const auto& msg = *maybe_msg; + total_labels = msg.names.size(); + invalid_labels.insert(msg.invalid.begin(), msg.invalid.end()); + for (size_t i = 0; i < msg.names.size(); ++i) { + const auto& name = msg.names[i]; + if (name.empty()) { + continue; + } + + label_names[i] = name; + } + + const auto metadata = json::parse(msg.metadata); + if (metadata.count("object_labels")) { + metadata.at("object_labels").get_to(object_labels); + } + + if (metadata.count("dynamic_labels")) { + metadata.at("dynamic_labels").get_to(dynamic_labels); + } + + if (metadata.count("surface_places_labels")) { + metadata.at("surface_places_labels").get_to(surface_places_labels); + } +} + +void declare_config(RosLabelspace::Config& config) { + using namespace config; + name("RosLabelspace::Config"); + field(config.ns, "ns"); + field(config.warning_timeout_s, "warning_timeout_s", "s"); + field(config.error_timeout_s, "error_timeout_s", "s"); + field(config.latch_info_sub, "latch_info_sub"); + check(config.warning_timeout_s, GE, 0.0, "warning_timeout_s"); + check(config.error_timeout_s, GE, 0.0, "error_timeout_s"); +} + +} // namespace hydra From 3de20c8ef9f1e931170125adab171f5e5b4e97f3 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 3 Jul 2026 23:12:03 +0000 Subject: [PATCH 2/2] fix config defaults and logging --- hydra_ros/include/hydra_ros/input/ros_labelspace.h | 4 ++-- hydra_ros/src/input/ros_labelspace.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/hydra_ros/include/hydra_ros/input/ros_labelspace.h b/hydra_ros/include/hydra_ros/input/ros_labelspace.h index 643d7206..ab72ab51 100644 --- a/hydra_ros/include/hydra_ros/input/ros_labelspace.h +++ b/hydra_ros/include/hydra_ros/input/ros_labelspace.h @@ -41,13 +41,13 @@ class RosLabelspace : public Labelspace { public: struct Config { //! @brief Optional namespace to use when listening for labelspace message - std::string ns = ""; + std::string ns = "~"; //! @brief Amount of time before warning about missing labelspace (0 disables) double warning_timeout_s = 10.0; //! @brief Amount of time to wait before forcing Hydra to exit (0 disables) double error_timeout_s = 0.0; //! @brief Whether or not to use latching for subscriber - bool latch_info_sub = false; + bool latch_info_sub = true; } const config; explicit RosLabelspace(const Config& config); diff --git a/hydra_ros/src/input/ros_labelspace.cpp b/hydra_ros/src/input/ros_labelspace.cpp index 20c81f14..df48bb89 100644 --- a/hydra_ros/src/input/ros_labelspace.cpp +++ b/hydra_ros/src/input/ros_labelspace.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_ros/common/ros_labelspace.h" +#include "hydra_ros/input/ros_labelspace.h" #include #include @@ -72,12 +72,12 @@ RosLabelspace::RosLabelspace(const Config& config) : config(config) { while (!maybe_msg && rclcpp::ok()) { maybe_msg = ianvs::getSingleMessage(nh, "labelspace", true, qos, timeout); if (!maybe_msg) { - LOG(WARNING) << "Waiting for CameraInfo on topic '" << resolved_topic << "'"; + LOG(WARNING) << "Waiting for labelspace on topic '" << resolved_topic << "'"; } const auto diff = nh.now() - start; if (config.error_timeout_s && (diff.seconds() > config.error_timeout_s)) { - LOG(ERROR) << "Sensor intrinsics lookup timed out on '" << resolved_topic << "'"; + LOG(ERROR) << "labelspace lookup timed out on '" << resolved_topic << "'"; break; } }