GT Room sink (#47)

* GT Room sink

* Add topic name

* fixup

* Fixes for Nathan

* Remove unused variables

Author: GoldenZephyr

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

hydra_ros/include/hydra_ros/backend/gt_room_publisher.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <hydra/backend/update_rooms_functor.h>
+#include <hydra_visualizer/color/colormap_utilities.h>
+#include <ianvs/node_handle.h>
+
+#include <memory>
+
+#include <rclcpp/publisher.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace hydra {
+
+class GtRoomPublisher : public UpdateRoomsFunctor::Sink {
+ public:
+  struct Config {
+    std::string ns = "~gt_rooms";
+    visualizer::DiscreteColormap::Config colormap;
+  } 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<visualization_msgs::msg::MarkerArray>::SharedPtr room_publisher_;
+};
+
+void declare_config(GtRoomPublisher::Config& config);
+
+}  // namespace hydra

hydra_ros/src/backend/gt_room_publisher.cpp

@@ -0,0 +1,70 @@
+#include "hydra_ros/backend/gt_room_publisher.h"
+
+#include <config_utilities/config.h>
+#include <config_utilities/factory.h>
+#include <config_utilities/parsing/context.h>
+#include <config_utilities/printing.h>
+#include <config_utilities/validation.h>
+#include <glog/logging.h>
+
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace hydra {
+
+static const auto registration_ =
+    config::RegistrationWithConfig<UpdateRoomsFunctor::Sink,
+                                   GtRoomPublisher,
+                                   GtRoomPublisher::Config>("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<MarkerArray>("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";
+  MarkerArray ma;
+  auto& m = ma.markers.emplace_back();
+  m.action = m.DELETEALL;
+  int idx = 0;
+  int room_idx = 0;
+
+  auto colormap = visualizer::DiscreteColormap(config.colormap);
+
+  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.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 = visualizer::makeColorMsg(colormap.getColor(room_idx), 0.5);
+    }
+    ++room_idx;
+  }
+
+  room_publisher_->publish(ma);
+}
+
+}  // namespace hydra