From 4377352a2cf3864bcbf0580e87b15155f0ee59b7 Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Sat, 11 Apr 2026 03:01:15 +0000 Subject: [PATCH 1/2] 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 fd499b144d4fb592fe3bcd509466cb4eddf20810 Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Sat, 11 Apr 2026 22:59:11 +0000 Subject: [PATCH 2/2] 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;