Properly rotate GT rooms (#64)

* Properly rotate GT rooms

* Set gt room frame id from config

* drop header

---------

Co-authored-by: Nathan Hughes <nathan.h.hughes@gmail.com>

Author: GoldenZephyr

hydra_ros/include/hydra_ros/backend/gt_room_publisher.h

@@ -14,7 +14,8 @@ namespace hydra {
 class GtRoomPublisher : public UpdateRoomsFunctor::Sink {
  public:
   struct Config {
-    std::string ns = "~gt_rooms";
+    std::string ns = "~/gt_rooms";
+    std::string room_frame_id = "map";
     visualizer::DiscreteColormap::Config colormap;
   } const config;
 

hydra_ros/src/backend/gt_room_publisher.cpp

@@ -21,6 +21,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");
 }
 
@@ -35,7 +36,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;
@@ -47,12 +47,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 = config.room_frame_id;
       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();