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..c47db3e --- /dev/null +++ b/hydra_ros/include/hydra_ros/backend/gt_room_publisher.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#include + +#include + +#include +#include + +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::SharedPtr room_publisher_; +}; + +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..dad2b21 --- /dev/null +++ b/hydra_ros/src/backend/gt_room_publisher.cpp @@ -0,0 +1,70 @@ +#include "hydra_ros/backend/gt_room_publisher.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +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); +} + +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