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..ab72ab51 --- /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 = true; + } 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..df48bb89 --- /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/input/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 labelspace on topic '" << resolved_topic << "'"; + } + + const auto diff = nh.now() - start; + if (config.error_timeout_s && (diff.seconds() > config.error_timeout_s)) { + LOG(ERROR) << "labelspace 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