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..3e0b23f 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,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; diff --git a/hydra_ros/src/backend/gt_room_publisher.cpp b/hydra_ros/src/backend/gt_room_publisher.cpp index dad2b21..a4fd38d 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 @@ -21,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"); } @@ -35,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; @@ -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 = 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();