From 3dbf7385c3486c0faac64c31823ecded51d01417 Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Sat, 11 Apr 2026 03:01:15 +0000 Subject: [PATCH 1/3] Properly rotate GT rooms --- .../include/hydra_ros/backend/gt_room_publisher.h | 2 +- hydra_ros/src/backend/gt_room_publisher.cpp | 10 ++++++++-- 2 files changed, 9 insertions(+), 3 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 c47db3e..2a31633 100644 --- a/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h +++ b/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h @@ -14,7 +14,7 @@ namespace hydra { class GtRoomPublisher : public UpdateRoomsFunctor::Sink { public: struct Config { - std::string ns = "~gt_rooms"; + std::string ns = "~/gt_rooms"; visualizer::DiscreteColormap::Config colormap; } const config; diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index dad2b21..685d837 100644 --- a/hydra_ros/src/backend/gt_room_publisher.cpp +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -1,5 +1,6 @@ #include "hydra_ros/backend/gt_room_publisher.h" +#include #include #include #include @@ -47,12 +48,17 @@ void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { for (auto room : rf.room_extents.room_bounding_boxes) { for (auto box : room) { auto& m = ma.markers.emplace_back(); - m.header.frame_id = "map"; + m.header.frame_id = "euclid/utm_local"; m.ns = "gt_rooms"; m.id = idx++; m.action = m.ADD; m.type = m.CUBE; - m.pose.orientation.w = 1; + + Eigen::Quaternionf q(box.world_R_center); + m.pose.orientation.x = q.x(); + m.pose.orientation.y = q.y(); + m.pose.orientation.z = q.z(); + m.pose.orientation.w = q.w(); 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(); From 893f2c0c7adf534ec3ee0cc5647fed177d23138d Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Sat, 11 Apr 2026 22:59:11 +0000 Subject: [PATCH 2/3] Set gt room frame id from config --- hydra_ros/include/hydra_ros/backend/gt_room_publisher.h | 1 + hydra_ros/src/backend/gt_room_publisher.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 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 2a31633..3e0b23f 100644 --- a/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h +++ b/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h @@ -15,6 +15,7 @@ class GtRoomPublisher : public UpdateRoomsFunctor::Sink { public: struct Config { std::string ns = "~/gt_rooms"; + std::string room_frame_id = "map"; visualizer::DiscreteColormap::Config colormap; } const config; diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index 685d837..a4fd38d 100644 --- a/hydra_ros/src/backend/gt_room_publisher.cpp +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -22,6 +22,7 @@ void declare_config(GtRoomPublisher::Config& config) { using namespace config; name("GtRoomPublisher::Config"); field(config.ns, "ns"); + field(config.room_frame_id, "room_frame_id"); field(config.colormap, "colormap"); } @@ -36,7 +37,6 @@ GtRoomPublisher::GtRoomPublisher(const Config& config) std::string GtRoomPublisher::printInfo() const { return config::toString(config); } void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { - LOG(WARNING) << "GT Room sink called"; MarkerArray ma; auto& m = ma.markers.emplace_back(); m.action = m.DELETEALL; @@ -48,7 +48,7 @@ void GtRoomPublisher::call(uint64_t, const RoomFinder& rf) const { for (auto room : rf.room_extents.room_bounding_boxes) { for (auto box : room) { auto& m = ma.markers.emplace_back(); - m.header.frame_id = "euclid/utm_local"; + m.header.frame_id = config.room_frame_id; m.ns = "gt_rooms"; m.id = idx++; m.action = m.ADD; From e45f1ea38f130364fd10b951191e0d3932bd5f1a Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 12 Jun 2026 01:40:49 +0000 Subject: [PATCH 3/3] drop header --- hydra_ros/src/backend/gt_room_publisher.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index a4fd38d..cf03a3f 100644 --- a/hydra_ros/src/backend/gt_room_publisher.cpp +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -1,6 +1,5 @@ #include "hydra_ros/backend/gt_room_publisher.h" -#include #include #include #include