From e234b9d6d6f770b8ebd950f40a4cc21a42fdbaf5 Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Wed, 19 Nov 2025 15:29:49 +0000 Subject: [PATCH 1/5] GT Room sink --- hydra_ros/CMakeLists.txt | 1 + .../hydra_ros/backend/gt_room_publisher.h | 38 ++++++++++ hydra_ros/src/backend/gt_room_publisher.cpp | 71 +++++++++++++++++++ 3 files changed, 110 insertions(+) create mode 100644 hydra_ros/include/hydra_ros/backend/gt_room_publisher.h create mode 100644 hydra_ros/src/backend/gt_room_publisher.cpp diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index 7ff6112..32a0bfd 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -36,6 +36,7 @@ add_library( src/active_window/reconstruction_visualizer.cpp src/active_window/tsdf_occupancy_publisher.cpp src/backend/ros_backend_publisher.cpp + src/backend/gt_room_publisher.cpp src/frontend/gvd_occupancy_publisher.cpp src/frontend/gvd_visualization_utilities.cpp src/frontend/object_visualizer.cpp diff --git a/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h b/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h new file mode 100644 index 0000000..b71cd05 --- /dev/null +++ b/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include + +#include + +#include +#include + +namespace hydra { + +class GtRoomPublisher : public UpdateRoomsFunctor::Sink { + public: + struct Config { + std::string ns = "~gt_rooms"; + } const config; + + explicit GtRoomPublisher(const Config&); + + virtual ~GtRoomPublisher() = default; + + std::string printInfo() const; + + void call(uint64_t timestamp_ns, const RoomFinder&) const; + + private: + ianvs::NodeHandle nh_; + rclcpp::Publisher::SharedPtr room_publisher_; + + inline static const auto registration_ = + config::RegistrationWithConfig( + "GtRoomPublisher"); +}; + +void declare_config(GtRoomPublisher::Config& config); + +} // namespace hydra diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp new file mode 100644 index 0000000..7b2b558 --- /dev/null +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -0,0 +1,71 @@ +#include "hydra_ros/backend/gt_room_publisher.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace hydra { + +void declare_config(GtRoomPublisher::Config& config) { + using namespace config; + name("GtRoomPublisher::Config"); + field(config.ns, "ns"); +} + +GtRoomPublisher::GtRoomPublisher(const Config& config) + : config(config), nh_(ianvs::NodeHandle::this_node(config.ns)) { + room_publisher_ = + nh_.create_publisher("topic_name", 1); +} + +std::string GtRoomPublisher::printInfo() const { return config::toString(config); } + +void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { + LOG(WARNING) << "GT Room sink called"; + visualization_msgs::msg::MarkerArray ma; + visualization_msgs::msg::Marker m; + m.action = m.DELETEALL; + ma.markers.push_back(m); + int idx = 0; + int room_idx = 0; + + const std::vector reds{0, .2, .4, .6, .8, 1}; + const std::vector greens{1, .8, .6, .4, .2}; + const std::vector blues{.4, .2, 0, 1}; + + for (auto room : rf.room_extents_.room_bounding_boxes) { + for (auto box : room) { + visualization_msgs::msg::Marker m; + m.header.frame_id = "map"; + m.ns = "gt_rooms"; + m.id = idx++; + m.action = m.ADD; + m.type = m.CUBE; + m.pose.orientation.w = 1; + m.pose.position.x = box.world_P_center.x(); + m.pose.position.y = box.world_P_center.y(); + m.pose.position.z = box.world_P_center.z(); + m.scale.x = box.dimensions.x(); + m.scale.y = box.dimensions.y(); + m.scale.z = box.dimensions.z(); + m.color.a = 0.5; + m.color.r = reds.at(room_idx % reds.size()); + m.color.g = greens.at(room_idx % greens.size()); + m.color.b = blues.at(room_idx % blues.size()); + ma.markers.push_back(m); + } + ++room_idx; + } + + room_publisher_->publish(ma); +} + +} // namespace hydra From 7db7e99ed4a54101072dc276a359a6456cdf46fa Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Wed, 19 Nov 2025 15:33:32 +0000 Subject: [PATCH 2/5] Add topic name --- hydra_ros/src/backend/gt_room_publisher.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index 7b2b558..4be0212 100644 --- a/hydra_ros/src/backend/gt_room_publisher.cpp +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -7,8 +7,6 @@ #include #include -#include - #include #include @@ -22,8 +20,8 @@ void declare_config(GtRoomPublisher::Config& config) { GtRoomPublisher::GtRoomPublisher(const Config& config) : config(config), nh_(ianvs::NodeHandle::this_node(config.ns)) { - room_publisher_ = - nh_.create_publisher("topic_name", 1); + room_publisher_ = nh_.create_publisher( + "gt_room_boundaries", 1); } std::string GtRoomPublisher::printInfo() const { return config::toString(config); } From d88cab0674c6dabf7437c3b555e212cdbae843cb Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Wed, 19 Nov 2025 16:01:01 +0000 Subject: [PATCH 3/5] fixup --- hydra_ros/src/backend/gt_room_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index 4be0212..3949525 100644 --- a/hydra_ros/src/backend/gt_room_publisher.cpp +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -39,7 +39,7 @@ void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { const std::vector greens{1, .8, .6, .4, .2}; const std::vector blues{.4, .2, 0, 1}; - for (auto room : rf.room_extents_.room_bounding_boxes) { + for (auto room : rf.room_extents.room_bounding_boxes) { for (auto box : room) { visualization_msgs::msg::Marker m; m.header.frame_id = "map"; From 74b888da7c4942a2b55ee35129dfcf486fd640a7 Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Wed, 19 Nov 2025 20:44:01 +0000 Subject: [PATCH 4/5] Fixes for Nathan --- .../hydra_ros/backend/gt_room_publisher.h | 6 ++--- hydra_ros/src/backend/gt_room_publisher.cpp | 27 +++++++++++-------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h b/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h index b71cd05..c47db3e 100644 --- a/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h +++ b/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -14,6 +15,7 @@ class GtRoomPublisher : public UpdateRoomsFunctor::Sink { public: struct Config { std::string ns = "~gt_rooms"; + visualizer::DiscreteColormap::Config colormap; } const config; explicit GtRoomPublisher(const Config&); @@ -27,10 +29,6 @@ class GtRoomPublisher : public UpdateRoomsFunctor::Sink { private: ianvs::NodeHandle nh_; rclcpp::Publisher::SharedPtr room_publisher_; - - inline static const auto registration_ = - config::RegistrationWithConfig( - "GtRoomPublisher"); }; void declare_config(GtRoomPublisher::Config& config); diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index 3949525..2160869 100644 --- a/hydra_ros/src/backend/gt_room_publisher.cpp +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -12,26 +12,33 @@ namespace hydra { +static const auto registration_ = + config::RegistrationWithConfig("GtRoomPublisher"); + void declare_config(GtRoomPublisher::Config& config) { using namespace config; name("GtRoomPublisher::Config"); field(config.ns, "ns"); + field(config.colormap, "colormap"); } +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + GtRoomPublisher::GtRoomPublisher(const Config& config) : config(config), nh_(ianvs::NodeHandle::this_node(config.ns)) { - room_publisher_ = nh_.create_publisher( - "gt_room_boundaries", 1); + room_publisher_ = nh_.create_publisher("gt_room_boundaries", 1); } std::string GtRoomPublisher::printInfo() const { return config::toString(config); } void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { LOG(WARNING) << "GT Room sink called"; - visualization_msgs::msg::MarkerArray ma; - visualization_msgs::msg::Marker m; + MarkerArray ma; + auto& m = ma.markers.emplace_back(); m.action = m.DELETEALL; - ma.markers.push_back(m); int idx = 0; int room_idx = 0; @@ -39,9 +46,11 @@ void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { const std::vector greens{1, .8, .6, .4, .2}; const std::vector blues{.4, .2, 0, 1}; + auto colormap = visualizer::DiscreteColormap(config.colormap); + for (auto room : rf.room_extents.room_bounding_boxes) { for (auto box : room) { - visualization_msgs::msg::Marker m; + auto& m = ma.markers.emplace_back(); m.header.frame_id = "map"; m.ns = "gt_rooms"; m.id = idx++; @@ -54,11 +63,7 @@ void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { m.scale.x = box.dimensions.x(); m.scale.y = box.dimensions.y(); m.scale.z = box.dimensions.z(); - m.color.a = 0.5; - m.color.r = reds.at(room_idx % reds.size()); - m.color.g = greens.at(room_idx % greens.size()); - m.color.b = blues.at(room_idx % blues.size()); - ma.markers.push_back(m); + m.color = visualizer::makeColorMsg(colormap.getColor(room_idx), 0.5); } ++room_idx; } From 62b775f1398b32603d5ba32d5230870cec9431eb Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Wed, 19 Nov 2025 20:59:09 +0000 Subject: [PATCH 5/5] Remove unused variables --- hydra_ros/src/backend/gt_room_publisher.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index 2160869..dad2b21 100644 --- a/hydra_ros/src/backend/gt_room_publisher.cpp +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -42,10 +42,6 @@ void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { int idx = 0; int room_idx = 0; - const std::vector reds{0, .2, .4, .6, .8, 1}; - const std::vector greens{1, .8, .6, .4, .2}; - const std::vector blues{.4, .2, 0, 1}; - auto colormap = visualizer::DiscreteColormap(config.colormap); for (auto room : rf.room_extents.room_bounding_boxes) {