From 1b3a4fd86767c64d0fc7baf1ef0e547782cd610f Mon Sep 17 00:00:00 2001 From: lschmid Date: Tue, 30 Sep 2025 16:19:50 -0400 Subject: [PATCH 1/8] add no semantic data receiver --- .../include/hydra_ros/input/image_receiver.h | 24 ++++++++++++ hydra_ros/src/input/image_receiver.cpp | 39 +++++++++++++++++++ 2 files changed, 63 insertions(+) diff --git a/hydra_ros/include/hydra_ros/input/image_receiver.h b/hydra_ros/include/hydra_ros/input/image_receiver.h index c81e786..4a1c130 100644 --- a/hydra_ros/include/hydra_ros/input/image_receiver.h +++ b/hydra_ros/include/hydra_ros/input/image_receiver.h @@ -182,6 +182,30 @@ void ImageReceiverImpl::callback( queue.push(packet); } +class NoSemanticImageReceiver : public RosDataReceiver { + public: + struct Config : RosDataReceiver::Config {}; + using Policy = + message_filters::sync_policies::ApproximateTime; + using Synchronizer = message_filters::Synchronizer; + + NoSemanticImageReceiver(const Config& config, const std::string& sensor_name); + virtual ~NoSemanticImageReceiver() = default; + + protected: + bool initImpl() override; + + void callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, + const sensor_msgs::msg::Image::ConstSharedPtr& depth); + + ColorSubscriber color_sub_; + DepthSubscriber depth_sub_; + std::unique_ptr sync_; +}; + +void declare_config(NoSemanticImageReceiver::Config& config); + class ClosedSetImageReceiver : public ImageReceiverImpl { public: struct Config : RosDataReceiver::Config {}; diff --git a/hydra_ros/src/input/image_receiver.cpp b/hydra_ros/src/input/image_receiver.cpp index c46e092..c13d1d5 100644 --- a/hydra_ros/src/input/image_receiver.cpp +++ b/hydra_ros/src/input/image_receiver.cpp @@ -130,6 +130,39 @@ void FeatureSubscriber::fillInput(const MsgType& msg, ImageInputPacket& packet) } } +void declare_config(NoSemanticImageReceiver::Config& config) { + using namespace config; + name("NoSemanticImageReceiver::Config"); + base(config); +} + +NoSemanticImageReceiver::NoSemanticImageReceiver(const Config& config, + const std::string& sensor_name) + : RosDataReceiver(config, sensor_name) {} + +bool NoSemanticImageReceiver::initImpl() { + color_sub_ = ColorSubscriber(ianvs::NodeHandle::this_node(ns_)); + depth_sub_ = DepthSubscriber(ianvs::NodeHandle::this_node(ns_)); + sync_.reset(new Synchronizer( + Policy(config.queue_size), color_sub_.getFilter(), depth_sub_.getFilter())); + sync_->registerCallback(&NoSemanticImageReceiver::callback, this); + return true; +} + +void NoSemanticImageReceiver::callback( + const sensor_msgs::msg::Image::ConstSharedPtr& color, + const sensor_msgs::msg::Image::ConstSharedPtr& depth) { + const auto timestamp_ns = rclcpp::Time(color->header.stamp).nanoseconds(); + if (!checkInputTimestamp(timestamp_ns)) { + return; + } + + auto packet = std::make_shared(timestamp_ns, sensor_name_); + color_sub_.fillInput(*color, *packet); + depth_sub_.fillInput(*depth, *packet); + queue.push(packet); +} + void declare_config(ClosedSetImageReceiver::Config& config) { using namespace config; name("ClosedSetImageReceiver::Config"); @@ -152,6 +185,12 @@ OpenSetImageReceiver::OpenSetImageReceiver(const Config& config, namespace { +static const auto no_semantic_registration = + config::RegistrationWithConfig("NoSemanticImageReceiver"); + static const auto closed_registration = config::RegistrationWithConfig Date: Tue, 30 Sep 2025 21:59:38 -0400 Subject: [PATCH 2/8] add single channel grayscale parsing for color inputs --- hydra_ros/src/input/image_receiver.cpp | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/hydra_ros/src/input/image_receiver.cpp b/hydra_ros/src/input/image_receiver.cpp index c13d1d5..1b8e286 100644 --- a/hydra_ros/src/input/image_receiver.cpp +++ b/hydra_ros/src/input/image_receiver.cpp @@ -56,11 +56,28 @@ ColorSubscriber::Filter& ColorSubscriber::getFilter() const { } void ColorSubscriber::fillInput(const Image& img, ImageInputPacket& packet) const { - try { - packet.color = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image; - } catch (const cv_bridge::Exception& e) { - LOG(ERROR) << "Failed to convert color image: " << e.what(); + // Allow also mono images to be converted to grayscale. + if (sensor_msgs::image_encodings::isColor(img.encoding)) { + try { + packet.color = + cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image; + return; + } catch (const cv_bridge::Exception& e) { + LOG(ERROR) << "Failed to convert mono image as color input: " << e.what(); + return; + } + } else if (sensor_msgs::image_encodings::isMono(img.encoding)) { + try { + cv::Mat mono = + cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8)->image; + cv::cvtColor(mono, packet.color, cv::COLOR_GRAY2RGB); + return; + } catch (const cv_bridge::Exception& e) { + LOG(ERROR) << "Failed to convert color image: " << e.what(); + return; + } } + LOG(ERROR) << "Failed to convert color image: unsupported encoding: " << img.encoding; } DepthSubscriber::DepthSubscriber() = default; From af4414ab6a232c43706ae5d22df2a03951a3bc4b Mon Sep 17 00:00:00 2001 From: lschmid Date: Sun, 5 Oct 2025 17:39:33 -0400 Subject: [PATCH 3/8] astro place subscriber --- hydra_ros/CMakeLists.txt | 1 + .../places/astro_traversability_estimator.h | 80 ++++++++++ .../places/astro_traversability_estimator.cpp | 151 ++++++++++++++++++ .../plugins/khronos_object_plugin.h | 13 +- .../hydra_visualizer/plugins/mesh_plugin.h | 14 +- .../src/plugins/khronos_object_plugin.cpp | 18 ++- hydra_visualizer/src/plugins/mesh_plugin.cpp | 29 +--- 7 files changed, 260 insertions(+), 46 deletions(-) create mode 100644 hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h create mode 100644 hydra_ros/src/places/astro_traversability_estimator.cpp diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index c74387d..57f636f 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -49,6 +49,7 @@ add_library( src/input/ros_data_receiver.cpp src/input/ros_input_module.cpp src/input/ros_sensors.cpp + src/places/astro_traversability_estimator.cpp src/odometry/ros_pose_graph_tracker.cpp src/openset/ros_embedding_group.cpp src/utils/bow_subscriber.cpp diff --git a/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h b/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h new file mode 100644 index 0000000..1b09636 --- /dev/null +++ b/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h @@ -0,0 +1,80 @@ +/* ----------------------------------------------------------------------------- + * 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 + +#include + +#include +#include + +namespace hydra::places { + +/** + * @brief Takes in an external traversability map from astro. + */ +class AstroTraversabilityEstimator : public TraversabilityEstimator { + public: + struct Config { + //! @brief The height above the robot body to consider for traversability in meters. + std::string input_topic = "/local_cost_map"; + unsigned int queue_size = 1; + } const config; + + using State = spark_dsg::TraversabilityState; + + AstroTraversabilityEstimator(const Config& config); + ~AstroTraversabilityEstimator() override = default; + + void updateTraversability(const ActiveWindowOutput& msg, + const kimera_pgmo::MeshDelta& /* mesh_delta */, + const spark_dsg::DynamicSceneGraph& /* graph */) override; + + void callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + + const TraversabilityLayer& getTraversabilityLayer() const override; + + protected: + rclcpp::Subscription::SharedPtr sub_; + mutable std::mutex mutex_; + + protected: + State occupancyToTraversability(int8_t occupancy) const; +}; + +void declare_config(AstroTraversabilityEstimator::Config& config); + +} // namespace hydra::places diff --git a/hydra_ros/src/places/astro_traversability_estimator.cpp b/hydra_ros/src/places/astro_traversability_estimator.cpp new file mode 100644 index 0000000..0307c71 --- /dev/null +++ b/hydra_ros/src/places/astro_traversability_estimator.cpp @@ -0,0 +1,151 @@ +/* ----------------------------------------------------------------------------- + * 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/places/astro_traversability_estimator.h" + +#include +#include +#include +#include + +namespace hydra::places { + +using spark_dsg::TraversabilityState; + +static const auto registration = + config::RegistrationWithConfig( + "AstroTraversabilityEstimator"); + +void declare_config(AstroTraversabilityEstimator::Config& config) { + using namespace config; + name("AstroTraversabilityEstimator::Config"); + field(config.input_topic, "input_topic"); + field(config.queue_size, "queue_size"); + checkCondition(!config.input_topic.empty(), "'input_topic' must be specified"); +} + +AstroTraversabilityEstimator::AstroTraversabilityEstimator(const Config& config) + : config(config::checkValid(config)) { + auto nh = ianvs::NodeHandle::this_node(); + sub_ = nh.create_subscription( + config.input_topic, + config.queue_size, + &AstroTraversabilityEstimator::callback, + this); +} + +const TraversabilityLayer& AstroTraversabilityEstimator::getTraversabilityLayer() + const { + std::lock_guard lock(mutex_); + return *traversability_layer_; +} + +void AstroTraversabilityEstimator::updateTraversability( + const ActiveWindowOutput& msg, + const kimera_pgmo::MeshDelta&, + const spark_dsg::DynamicSceneGraph&) { + if (!traversability_layer_) { + const auto& map_config = msg.map().config; + traversability_layer_ = std::make_unique( + map_config.voxel_size, map_config.voxels_per_side); + } +} + +void AstroTraversabilityEstimator::callback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + if (!traversability_layer_) { + return; + } + // Reset update tracking. + std::lock_guard lock(mutex_); + for (auto& block : *traversability_layer_) { + block.updated = false; + for (auto& voxel : block.voxels) { + voxel.confidence = 0.0f; + } + } + + // Updated the stored layer with the map. + const float voxel_size = msg->info.resolution; + const Eigen::Vector3f origin( + msg->info.origin.position.x, msg->info.origin.position.y, 0.0f); + for (unsigned int y = 0; y < msg->info.height; ++y) { + for (unsigned int x = 0; x < msg->info.width; ++x) { + const int idx = x + y * msg->info.width; + const State state = occupancyToTraversability(msg->data[idx]); + // Compute the voxel coordinates. + const Eigen::Vector3f position = + Eigen::Vector3f(x + 0.5f, y + 0.5f, 0.0f) * voxel_size + origin; + const auto index = traversability_layer_->globalIndexFromPoint(position); + // TODO(lschmid): Fix the allocate block interfaces for the traversability layer. + auto& block = traversability_layer_->allocateBlock( + traversability_layer_->blockIndexFromGlobal(index), + traversability_layer_->voxels_per_side); + block.updated = true; + auto& voxel = block.voxel(traversability_layer_->voxelIndexFromGlobal(index)); + if (voxel.confidence == 0.0f) { + // Overwrite if this is the first observation this iteration. + voxel.state = state; + voxel.confidence = 1.0f; + } else { + spark_dsg::fuseStates(state, voxel.state); + voxel.confidence += 1.0f; + } + } + } + + // Remove blocks that were not updated. + spatial_hash::BlockIndices to_remove; + for (auto& block : *traversability_layer_) { + if (!block.updated) { + to_remove.push_back(block.index); + } + } + traversability_layer_->removeBlocks(to_remove); +} + +AstroTraversabilityEstimator::State +AstroTraversabilityEstimator::occupancyToTraversability(int8_t occupancy) const { + if (occupancy < 0) { + return State::UNKNOWN; + } + if (occupancy == 0) { + return State::TRAVERSABLE; + } + return State::INTRAVERSABLE; +} + +} // namespace hydra::places diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h index 8542bc1..f959fa9 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h @@ -73,6 +73,7 @@ #pragma once +#include #include #include #include @@ -122,7 +123,7 @@ class KhronosObjectPlugin : public VisualizerPlugin { //! Layer to draw objects for std::string layer = spark_dsg::DsgLayers::OBJECTS; - } const config; + }; // Construction. KhronosObjectPlugin(const Config& config, @@ -136,9 +137,11 @@ class KhronosObjectPlugin : public VisualizerPlugin { protected: // Helper functions. - void drawDynamicObjects(const std_msgs::msg::Header& header, + void drawDynamicObjects(const Config& config, + const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph); - void drawStaticObjects(const std_msgs::msg::Header& header, + void drawStaticObjects(const Config& config, + const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph); // Mesh namespace for the visualizer plugin. @@ -153,13 +156,15 @@ class KhronosObjectPlugin : public VisualizerPlugin { const uint64_t id); // Get the color of a dynamic object. - spark_dsg::Color getDynamicColor(const spark_dsg::KhronosObjectAttributes& attrs, + spark_dsg::Color getDynamicColor(const Config& config, + const spark_dsg::KhronosObjectAttributes& attrs, const uint64_t id) const; // Clear static object visuliazation void resetObject(const std_msgs::msg::Header& header, const uint64_t id); private: + const config::DynamicConfig config_; // ROS. rclcpp::Publisher::SharedPtr dynamic_pub_; rclcpp::Publisher::SharedPtr static_pub_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h index 70e0cdc..a68f060 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h @@ -33,13 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once +#include #include #include #include #include -#include -#include #include "hydra_visualizer/adapters/mesh_color.h" #include "hydra_visualizer/plugins/visualizer_plugin.h" @@ -52,9 +51,8 @@ class MeshPlugin : public VisualizerPlugin { using LabelsPtr = std::shared_ptr; struct Config { - bool use_color_adapter = false; - config::VirtualConfig coloring{SemanticMeshColoring::Config()}; - } const config; + config::VirtualConfig coloring; + }; MeshPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name); @@ -66,15 +64,11 @@ class MeshPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; protected: - void handleService(const std_srvs::srv::SetBool::Request::SharedPtr& req, - std_srvs::srv::SetBool::Response::SharedPtr res); + config::DynamicConfig config_; std::string getMsgNamespace() const; - bool use_color_adapter_; rclcpp::Publisher::SharedPtr mesh_pub_; - rclcpp::Service::SharedPtr toggle_service_; - std::shared_ptr mesh_coloring_; }; void declare_config(MeshPlugin::Config& config); diff --git a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp index b048848..5261eb7 100644 --- a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp +++ b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp @@ -122,7 +122,7 @@ KhronosObjectPlugin::KhronosObjectPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name) : VisualizerPlugin(name), - config(config::checkValid(config)), + config_("KhronosObjectPlugin", config::checkValid(config)), dynamic_pub_( nh.create_publisher("dynamic_objects", config.queue_size)), static_pub_(nh.create_publisher("static_objects", @@ -131,11 +131,12 @@ KhronosObjectPlugin::KhronosObjectPlugin(const Config& config, void KhronosObjectPlugin::draw(const std_msgs::msg::Header& header, const DynamicSceneGraph& graph) { + const auto config = config_.get(); if (!graph.hasLayer(config.layer)) { return; } - drawDynamicObjects(header, graph); - drawStaticObjects(header, graph); + drawDynamicObjects(config, header, graph); + drawStaticObjects(config, header, graph); } void KhronosObjectPlugin::reset(const std_msgs::msg::Header& header) { @@ -153,7 +154,8 @@ void KhronosObjectPlugin::reset(const std_msgs::msg::Header& header) { previous_objects_.clear(); } -void KhronosObjectPlugin::drawDynamicObjects(const std_msgs::msg::Header& header, +void KhronosObjectPlugin::drawDynamicObjects(const Config& config, + const std_msgs::msg::Header& header, const DynamicSceneGraph& graph) { if (dynamic_pub_->get_subscription_count() == 0) { return; @@ -171,7 +173,7 @@ void KhronosObjectPlugin::drawDynamicObjects(const std_msgs::msg::Header& header const uint64_t id = spark_dsg::NodeSymbol(node_id).categoryId(); spark_dsg::BoundingBox bbox = attrs->bounding_box; - const auto color = visualizer::makeColorMsg(getDynamicColor(*attrs, id)); + const auto color = visualizer::makeColorMsg(getDynamicColor(config, *attrs, id)); Marker bbox_template; bbox_template.type = Marker::LINE_LIST; @@ -220,7 +222,8 @@ void KhronosObjectPlugin::drawDynamicObjects(const std_msgs::msg::Header& header dynamic_pub_->publish(msg); } -void KhronosObjectPlugin::drawStaticObjects(const std_msgs::msg::Header& header, +void KhronosObjectPlugin::drawStaticObjects(const Config& config, + const std_msgs::msg::Header& header, const DynamicSceneGraph& dsg) { if (static_pub_->get_subscription_count() == 0) { return; @@ -295,7 +298,8 @@ void KhronosObjectPlugin::publishTransform(const std_msgs::msg::Header& header, tf_broadcaster_.sendTransform(msg); } -Color KhronosObjectPlugin::getDynamicColor(const KhronosObjectAttributes& attrs, +Color KhronosObjectPlugin::getDynamicColor(const Config& config, + const KhronosObjectAttributes& attrs, const uint64_t id) const { switch (config.dynamic_color_mode) { case Config::DynamicColorMode::ID: diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index e0b164c..dffffe3 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_plugin.cpp @@ -58,10 +58,6 @@ using spark_dsg::DynamicSceneGraph; void declare_config(MeshPlugin::Config& config) { using namespace config; name("MeshPlugin::Config"); - // TODO(lschmid): Not the most elegant, would be nice to dynamically set different - // colorings once config_utilities dynamic config is ready. - field(config.use_color_adapter, "use_color_adapter"); - config.coloring.setOptional(); field(config.coloring, "coloring"); } @@ -69,16 +65,9 @@ MeshPlugin::MeshPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name) : VisualizerPlugin(name), - config(config::checkValid(config)), - use_color_adapter_(config.use_color_adapter), + config_("MeshPlugin", config::checkValid(config)), mesh_pub_(nh.create_publisher( - name, rclcpp::QoS(1).transient_local())), - mesh_coloring_(config.coloring.create()) { - if (mesh_coloring_) { - toggle_service_ = (nh / name).create_service( - "use_color_adapter", &MeshPlugin::handleService, this); - } -} + name, rclcpp::QoS(1).transient_local())) {} MeshPlugin::~MeshPlugin() {} @@ -89,13 +78,10 @@ void MeshPlugin::draw(const std_msgs::msg::Header& header, return; } - if (use_color_adapter_ && !mesh_coloring_) { - LOG(WARNING) - << "[MeshPlugin] Invalid colormap; defaulting to original vertex color"; - } + const auto config = config_.get(); auto msg = visualizer::makeMeshMsg( - header, *mesh, getMsgNamespace(), use_color_adapter_ ? mesh_coloring_ : nullptr); + header, *mesh, getMsgNamespace(), config.coloring.create()); mesh_pub_->publish(msg); } @@ -111,11 +97,4 @@ std::string MeshPlugin::getMsgNamespace() const { return "robot0/dsg_mesh"; } -void MeshPlugin::handleService(const std_srvs::srv::SetBool::Request::SharedPtr& req, - std_srvs::srv::SetBool::Response::SharedPtr res) { - use_color_adapter_ = req->data; - res->success = true; - has_change_ = true; -} - } // namespace hydra From 254310caa0a1939e68111339a8d30a2e98942af6 Mon Sep 17 00:00:00 2001 From: lschmid Date: Mon, 6 Oct 2025 14:07:51 -0400 Subject: [PATCH 4/8] play around with fixing astro input relative pose --- .../include/hydra_ros/places/astro_traversability_estimator.h | 2 +- hydra_ros/src/frontend/traversability_visualizer.cpp | 2 +- hydra_ros/src/places/astro_traversability_estimator.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h b/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h index 1b09636..9b22832 100644 --- a/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h +++ b/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h @@ -51,7 +51,7 @@ class AstroTraversabilityEstimator : public TraversabilityEstimator { struct Config { //! @brief The height above the robot body to consider for traversability in meters. std::string input_topic = "/local_cost_map"; - unsigned int queue_size = 1; + unsigned int queue_size = 2; } const config; using State = spark_dsg::TraversabilityState; diff --git a/hydra_ros/src/frontend/traversability_visualizer.cpp b/hydra_ros/src/frontend/traversability_visualizer.cpp index 13f93d2..968edcc 100644 --- a/hydra_ros/src/frontend/traversability_visualizer.cpp +++ b/hydra_ros/src/frontend/traversability_visualizer.cpp @@ -173,7 +173,7 @@ Color TraversabilityVisualizer::debugColor(float value) const { if (value < 0.0f) { return Color::black(); } - return spark_dsg::colormaps::rainbowId(static_cast(value), 5); + return spark_dsg::colormaps::rainbowId(static_cast(value), 10); } } // namespace hydra diff --git a/hydra_ros/src/places/astro_traversability_estimator.cpp b/hydra_ros/src/places/astro_traversability_estimator.cpp index 0307c71..ce6b44c 100644 --- a/hydra_ros/src/places/astro_traversability_estimator.cpp +++ b/hydra_ros/src/places/astro_traversability_estimator.cpp @@ -108,7 +108,7 @@ void AstroTraversabilityEstimator::callback( const State state = occupancyToTraversability(msg->data[idx]); // Compute the voxel coordinates. const Eigen::Vector3f position = - Eigen::Vector3f(x + 0.5f, y + 0.5f, 0.0f) * voxel_size + origin; + Eigen::Vector3f(x, y, 0.0f) * voxel_size + origin; const auto index = traversability_layer_->globalIndexFromPoint(position); // TODO(lschmid): Fix the allocate block interfaces for the traversability layer. auto& block = traversability_layer_->allocateBlock( From 8a809f7acdb61b4f7ceb36c1abd75972eb288929 Mon Sep 17 00:00:00 2001 From: lschmid Date: Wed, 8 Oct 2025 11:34:09 -0400 Subject: [PATCH 5/8] rename traversability estimator --- .../places/astro_traversability_estimator.h | 11 +++++---- .../places/astro_traversability_estimator.cpp | 24 +++++++++---------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h b/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h index 9b22832..a8cea81 100644 --- a/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h +++ b/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h @@ -44,9 +44,10 @@ namespace hydra::places { /** - * @brief Takes in an external traversability map from astro. + * @brief Takes in an external traversability map as occupancy grid to perform + * traversability analysis. */ -class AstroTraversabilityEstimator : public TraversabilityEstimator { +class ExternalTraversabilityEstimator : public TraversabilityEstimator { public: struct Config { //! @brief The height above the robot body to consider for traversability in meters. @@ -56,8 +57,8 @@ class AstroTraversabilityEstimator : public TraversabilityEstimator { using State = spark_dsg::TraversabilityState; - AstroTraversabilityEstimator(const Config& config); - ~AstroTraversabilityEstimator() override = default; + ExternalTraversabilityEstimator(const Config& config); + ~ExternalTraversabilityEstimator() override = default; void updateTraversability(const ActiveWindowOutput& msg, const kimera_pgmo::MeshDelta& /* mesh_delta */, @@ -75,6 +76,6 @@ class AstroTraversabilityEstimator : public TraversabilityEstimator { State occupancyToTraversability(int8_t occupancy) const; }; -void declare_config(AstroTraversabilityEstimator::Config& config); +void declare_config(ExternalTraversabilityEstimator::Config& config); } // namespace hydra::places diff --git a/hydra_ros/src/places/astro_traversability_estimator.cpp b/hydra_ros/src/places/astro_traversability_estimator.cpp index ce6b44c..7238ae4 100644 --- a/hydra_ros/src/places/astro_traversability_estimator.cpp +++ b/hydra_ros/src/places/astro_traversability_estimator.cpp @@ -45,35 +45,35 @@ using spark_dsg::TraversabilityState; static const auto registration = config::RegistrationWithConfig( - "AstroTraversabilityEstimator"); + ExternalTraversabilityEstimator, + ExternalTraversabilityEstimator::Config>( + "ExternalTraversabilityEstimator"); -void declare_config(AstroTraversabilityEstimator::Config& config) { +void declare_config(ExternalTraversabilityEstimator::Config& config) { using namespace config; - name("AstroTraversabilityEstimator::Config"); + name("ExternalTraversabilityEstimator::Config"); field(config.input_topic, "input_topic"); field(config.queue_size, "queue_size"); checkCondition(!config.input_topic.empty(), "'input_topic' must be specified"); } -AstroTraversabilityEstimator::AstroTraversabilityEstimator(const Config& config) +ExternalTraversabilityEstimator::ExternalTraversabilityEstimator(const Config& config) : config(config::checkValid(config)) { auto nh = ianvs::NodeHandle::this_node(); sub_ = nh.create_subscription( config.input_topic, config.queue_size, - &AstroTraversabilityEstimator::callback, + &ExternalTraversabilityEstimator::callback, this); } -const TraversabilityLayer& AstroTraversabilityEstimator::getTraversabilityLayer() +const TraversabilityLayer& ExternalTraversabilityEstimator::getTraversabilityLayer() const { std::lock_guard lock(mutex_); return *traversability_layer_; } -void AstroTraversabilityEstimator::updateTraversability( +void ExternalTraversabilityEstimator::updateTraversability( const ActiveWindowOutput& msg, const kimera_pgmo::MeshDelta&, const spark_dsg::DynamicSceneGraph&) { @@ -84,7 +84,7 @@ void AstroTraversabilityEstimator::updateTraversability( } } -void AstroTraversabilityEstimator::callback( +void ExternalTraversabilityEstimator::callback( const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { if (!traversability_layer_) { return; @@ -137,8 +137,8 @@ void AstroTraversabilityEstimator::callback( traversability_layer_->removeBlocks(to_remove); } -AstroTraversabilityEstimator::State -AstroTraversabilityEstimator::occupancyToTraversability(int8_t occupancy) const { +ExternalTraversabilityEstimator::State +ExternalTraversabilityEstimator::occupancyToTraversability(int8_t occupancy) const { if (occupancy < 0) { return State::UNKNOWN; } From cb331d1bdf4ec07003822ece1cdef96d6c5bd0a2 Mon Sep 17 00:00:00 2001 From: lschmid Date: Wed, 8 Oct 2025 11:36:43 -0400 Subject: [PATCH 6/8] rename files accordingly --- hydra_ros/CMakeLists.txt | 2 +- ...bility_estimator.h => external_traversability_estimator.h} | 0 ...ty_estimator.cpp => external_traversability_estimator.cpp} | 4 ++-- 3 files changed, 3 insertions(+), 3 deletions(-) rename hydra_ros/include/hydra_ros/places/{astro_traversability_estimator.h => external_traversability_estimator.h} (100%) rename hydra_ros/src/places/{astro_traversability_estimator.cpp => external_traversability_estimator.cpp} (98%) diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index 57f636f..7ff6112 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -49,7 +49,7 @@ add_library( src/input/ros_data_receiver.cpp src/input/ros_input_module.cpp src/input/ros_sensors.cpp - src/places/astro_traversability_estimator.cpp + src/places/external_traversability_estimator.cpp src/odometry/ros_pose_graph_tracker.cpp src/openset/ros_embedding_group.cpp src/utils/bow_subscriber.cpp diff --git a/hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h b/hydra_ros/include/hydra_ros/places/external_traversability_estimator.h similarity index 100% rename from hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h rename to hydra_ros/include/hydra_ros/places/external_traversability_estimator.h diff --git a/hydra_ros/src/places/astro_traversability_estimator.cpp b/hydra_ros/src/places/external_traversability_estimator.cpp similarity index 98% rename from hydra_ros/src/places/astro_traversability_estimator.cpp rename to hydra_ros/src/places/external_traversability_estimator.cpp index 7238ae4..eba519f 100644 --- a/hydra_ros/src/places/astro_traversability_estimator.cpp +++ b/hydra_ros/src/places/external_traversability_estimator.cpp @@ -32,13 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_ros/places/astro_traversability_estimator.h" - #include #include #include #include +#include "hydra_ros/places/external_traversability_estimator.h" + namespace hydra::places { using spark_dsg::TraversabilityState; From 65e2a052132ee4da46a5adbf04cb8922cb8f4d92 Mon Sep 17 00:00:00 2001 From: lschmid Date: Wed, 8 Oct 2025 13:34:46 -0400 Subject: [PATCH 7/8] run pre-commit --- hydra_ros/src/places/external_traversability_estimator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hydra_ros/src/places/external_traversability_estimator.cpp b/hydra_ros/src/places/external_traversability_estimator.cpp index eba519f..4f440b1 100644 --- a/hydra_ros/src/places/external_traversability_estimator.cpp +++ b/hydra_ros/src/places/external_traversability_estimator.cpp @@ -32,13 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ +#include "hydra_ros/places/external_traversability_estimator.h" + #include #include #include #include -#include "hydra_ros/places/external_traversability_estimator.h" - namespace hydra::places { using spark_dsg::TraversabilityState; From 7068118836ed5990a460ff9980337d92cdb19ddf Mon Sep 17 00:00:00 2001 From: lschmid Date: Wed, 8 Oct 2025 14:17:38 -0400 Subject: [PATCH 8/8] rename RGBD image receiver --- .../include/hydra_ros/input/image_receiver.h | 8 +++---- hydra_ros/src/input/image_receiver.cpp | 23 +++++++++---------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/hydra_ros/include/hydra_ros/input/image_receiver.h b/hydra_ros/include/hydra_ros/input/image_receiver.h index 4a1c130..04a224e 100644 --- a/hydra_ros/include/hydra_ros/input/image_receiver.h +++ b/hydra_ros/include/hydra_ros/input/image_receiver.h @@ -182,7 +182,7 @@ void ImageReceiverImpl::callback( queue.push(packet); } -class NoSemanticImageReceiver : public RosDataReceiver { +class RGBDImageReceiver : public RosDataReceiver { public: struct Config : RosDataReceiver::Config {}; using Policy = @@ -190,8 +190,8 @@ class NoSemanticImageReceiver : public RosDataReceiver { sensor_msgs::msg::Image>; using Synchronizer = message_filters::Synchronizer; - NoSemanticImageReceiver(const Config& config, const std::string& sensor_name); - virtual ~NoSemanticImageReceiver() = default; + RGBDImageReceiver(const Config& config, const std::string& sensor_name); + virtual ~RGBDImageReceiver() = default; protected: bool initImpl() override; @@ -204,7 +204,7 @@ class NoSemanticImageReceiver : public RosDataReceiver { std::unique_ptr sync_; }; -void declare_config(NoSemanticImageReceiver::Config& config); +void declare_config(RGBDImageReceiver::Config& config); class ClosedSetImageReceiver : public ImageReceiverImpl { public: diff --git a/hydra_ros/src/input/image_receiver.cpp b/hydra_ros/src/input/image_receiver.cpp index 1b8e286..85403a1 100644 --- a/hydra_ros/src/input/image_receiver.cpp +++ b/hydra_ros/src/input/image_receiver.cpp @@ -147,28 +147,27 @@ void FeatureSubscriber::fillInput(const MsgType& msg, ImageInputPacket& packet) } } -void declare_config(NoSemanticImageReceiver::Config& config) { +void declare_config(RGBDImageReceiver::Config& config) { using namespace config; - name("NoSemanticImageReceiver::Config"); + name("RGBDImageReceiver::Config"); base(config); } -NoSemanticImageReceiver::NoSemanticImageReceiver(const Config& config, - const std::string& sensor_name) +RGBDImageReceiver::RGBDImageReceiver(const Config& config, + const std::string& sensor_name) : RosDataReceiver(config, sensor_name) {} -bool NoSemanticImageReceiver::initImpl() { +bool RGBDImageReceiver::initImpl() { color_sub_ = ColorSubscriber(ianvs::NodeHandle::this_node(ns_)); depth_sub_ = DepthSubscriber(ianvs::NodeHandle::this_node(ns_)); sync_.reset(new Synchronizer( Policy(config.queue_size), color_sub_.getFilter(), depth_sub_.getFilter())); - sync_->registerCallback(&NoSemanticImageReceiver::callback, this); + sync_->registerCallback(&RGBDImageReceiver::callback, this); return true; } -void NoSemanticImageReceiver::callback( - const sensor_msgs::msg::Image::ConstSharedPtr& color, - const sensor_msgs::msg::Image::ConstSharedPtr& depth) { +void RGBDImageReceiver::callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, + const sensor_msgs::msg::Image::ConstSharedPtr& depth) { const auto timestamp_ns = rclcpp::Time(color->header.stamp).nanoseconds(); if (!checkInputTimestamp(timestamp_ns)) { return; @@ -204,9 +203,9 @@ namespace { static const auto no_semantic_registration = config::RegistrationWithConfig("NoSemanticImageReceiver"); + RGBDImageReceiver, + RGBDImageReceiver::Config, + std::string>("RGBDImageReceiver"); static const auto closed_registration = config::RegistrationWithConfig