From 1adc435b6a435a604ba0108c2c94640b7920d6b5 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 12 Dec 2025 15:47:36 +0000 Subject: [PATCH 1/5] fix trait usage in mesh adapter --- hydra_visualizer/src/drawing.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/hydra_visualizer/src/drawing.cpp b/hydra_visualizer/src/drawing.cpp index 06d1141..223b88f 100644 --- a/hydra_visualizer/src/drawing.cpp +++ b/hydra_visualizer/src/drawing.cpp @@ -648,12 +648,11 @@ kimera_pgmo_msgs::msg::Mesh makeMeshMsg(const std_msgs::msg::Header& header, MeshColorAdapter adapter(mesh, coloring); msg.vertices.resize(mesh.points.size()); - msg.vertex_colors.resize(mesh.points.size()); for (size_t i = 0; i < mesh.points.size(); ++i) { auto& vertex = msg.vertices[i]; - tf2::convert(mesh.points[i].cast().eval(), vertex); - auto& color = msg.vertex_colors[i]; - color = visualizer::makeColorMsg(adapter.getVertexColor(i)); + tf2::convert(mesh.points[i].cast().eval(), vertex.pos); + vertex.has_color = true; + vertex.color = visualizer::makeColorMsg(adapter.getVertexColor(i)); } msg.triangles.resize(mesh.faces.size()); From d11c7824890a19c6d605e6547633b37ab73c4ca5 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 12 Dec 2025 11:08:08 -0500 Subject: [PATCH 2/5] fix object visualizer --- .../hydra_ros/frontend/object_visualizer.h | 1 - hydra_ros/src/frontend/object_visualizer.cpp | 17 +++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/hydra_ros/include/hydra_ros/frontend/object_visualizer.h b/hydra_ros/include/hydra_ros/frontend/object_visualizer.h index 644cbd2..bddf006 100644 --- a/hydra_ros/include/hydra_ros/frontend/object_visualizer.h +++ b/hydra_ros/include/hydra_ros/frontend/object_visualizer.h @@ -58,7 +58,6 @@ class ObjectVisualizer : public MeshSegmenter::Sink { void call(uint64_t timestamp_ns, const kimera_pgmo::MeshDelta& delta, - const std::vector& active, const LabelIndices& label_indices) const override; protected: diff --git a/hydra_ros/src/frontend/object_visualizer.cpp b/hydra_ros/src/frontend/object_visualizer.cpp index 0783d90..4580025 100644 --- a/hydra_ros/src/frontend/object_visualizer.cpp +++ b/hydra_ros/src/frontend/object_visualizer.cpp @@ -61,7 +61,6 @@ std::string ObjectVisualizer::printInfo() const { return config::toString(config void ObjectVisualizer::call(uint64_t timestamp_ns, const kimera_pgmo::MeshDelta& delta, - const std::vector& active, const LabelIndices& label_indices) const { pubs_.publish("active_vertices", [&]() { auto msg = std::make_unique(); @@ -69,6 +68,8 @@ void ObjectVisualizer::call(uint64_t timestamp_ns, msg->header.frame_id = GlobalInfo::instance().getFrames().odom; msg->ns = "active_vertices"; msg->id = 0; + std::vector active(delta.getNumActiveVertices()); + std::iota(active.begin(), active.end(), delta.getNumArchivedVertices()); fillMarkerFromCloud(delta, active, *msg); return msg; }); @@ -101,15 +102,15 @@ void ObjectVisualizer::fillMarkerFromCloud(const kimera_pgmo::MeshDelta& delta, msg.points.reserve(indices.size()); msg.colors.reserve(indices.size()); for (const auto idx : indices) { - const auto& p = delta.vertex_updates->at(idx); + const auto& p = delta.getVertex(idx); auto& point = msg.points.emplace_back(); - point.x = p.x; - point.y = p.y; - point.z = p.z; + point.x = p.pos.x(); + point.y = p.pos.y(); + point.z = p.pos.z(); auto& color = msg.colors.emplace_back(); - color.r = p.r / 255.0f; - color.g = p.g / 255.0f; - color.b = p.b / 255.0f; + color.r = p.traits.color[0] / 255.0f; + color.g = p.traits.color[1] / 255.0f; + color.b = p.traits.color[2] / 255.0f; color.a = config.point_alpha; } } From e7929c1e70e9686bef7f29e183f9d56271b97c52 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 12 Dec 2025 16:20:27 -0500 Subject: [PATCH 3/5] fix frontend delta publishing --- .../frontend/ros_frontend_publisher.h | 18 ++++---- .../src/frontend/ros_frontend_publisher.cpp | 41 ++++++++++--------- 2 files changed, 31 insertions(+), 28 deletions(-) diff --git a/hydra_ros/include/hydra_ros/frontend/ros_frontend_publisher.h b/hydra_ros/include/hydra_ros/frontend/ros_frontend_publisher.h index 5ba2b48..3e6c88d 100644 --- a/hydra_ros/include/hydra_ros/frontend/ros_frontend_publisher.h +++ b/hydra_ros/include/hydra_ros/frontend/ros_frontend_publisher.h @@ -34,12 +34,11 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include #include #include -#include +#include #include #include "hydra_ros/utils/dsg_streaming_interface.h" @@ -49,6 +48,9 @@ namespace hydra { class RosFrontendPublisher : public GraphBuilder::Sink { public: using MeshDeltaSrv = kimera_pgmo_msgs::srv::MeshDeltaQuery; + using MeshDeltaRequest = kimera_pgmo_msgs::srv::MeshDeltaQuery::Request::SharedPtr; + using MeshDeltaResponse = kimera_pgmo_msgs::srv::MeshDeltaQuery::Response::SharedPtr; + using MeshDeltaMsg = kimera_pgmo_msgs::msg::MeshDelta; struct Config { //! @brief Configuration for dsg publisher @@ -65,16 +67,14 @@ class RosFrontendPublisher : public GraphBuilder::Sink { std::string printInfo() const override { return "RosFrontendPublisher"; } protected: - void processMeshDeltaQuery(const MeshDeltaSrv::Request::SharedPtr req, - MeshDeltaSrv::Response::SharedPtr resp); + void processMeshDeltaQuery(const MeshDeltaRequest req, MeshDeltaResponse resp); std::unique_ptr dsg_sender_; - mutable std::map stored_delta_; - pose_graph_tools::PoseGraphPublisher mesh_graph_pub_; - kimera_pgmo::PgmoMeshDeltaPublisher mesh_update_pub_; + rclcpp::Publisher::SharedPtr mesh_update_pub_; rclcpp::Service::SharedPtr mesh_delta_server_; - rclcpp::TypeAdapter - mesh_delta_converter_; + + mutable std::map stored_delta_; }; + } // namespace hydra diff --git a/hydra_ros/src/frontend/ros_frontend_publisher.cpp b/hydra_ros/src/frontend/ros_frontend_publisher.cpp index bfd0e69..5ae4d2e 100644 --- a/hydra_ros/src/frontend/ros_frontend_publisher.cpp +++ b/hydra_ros/src/frontend/ros_frontend_publisher.cpp @@ -39,10 +39,10 @@ #include #include #include +#include namespace hydra { -using kimera_pgmo::MeshDeltaTypeAdapter; using pose_graph_tools::PoseGraphTypeAdapter; using BaseInterface = rclcpp::node_interfaces::NodeBaseInterface; using rclcpp::CallbackGroupType; @@ -66,7 +66,8 @@ void declare_config(RosFrontendPublisher::Config& config) { } RosFrontendPublisher::RosFrontendPublisher(ianvs::NodeHandle nh) - : config(config::checkValid(get_config())) { + : config(config::checkValid(get_config())), + dsg_sender_(new DsgSender(config.dsg_sender, nh)) { auto group = nh.as()->create_callback_group( CallbackGroupType::MutuallyExclusive); mesh_delta_server_ = @@ -75,11 +76,11 @@ RosFrontendPublisher::RosFrontendPublisher(ianvs::NodeHandle nh) this, rclcpp::ServicesQoS(), group); - dsg_sender_ = std::make_unique(config.dsg_sender, nh); - mesh_graph_pub_ = nh.create_publisher( - "mesh_graph_incremental", rclcpp::QoS(100).transient_local()); - mesh_update_pub_ = nh.create_publisher( - "full_mesh_update", rclcpp::QoS(100).transient_local()); + + const auto qos = rclcpp::QoS(100).transient_local(); + mesh_graph_pub_ = + nh.create_publisher("mesh_graph_incremental", qos); + mesh_update_pub_ = nh.create_publisher("full_mesh_update", qos); } void RosFrontendPublisher::call(uint64_t timestamp_ns, @@ -87,13 +88,16 @@ void RosFrontendPublisher::call(uint64_t timestamp_ns, const BackendInput& backend_input) const { // TODO(nathan) make sure pgmo stamps the deformation graph mesh_graph_pub_->publish(backend_input.deformation_graph); + if (backend_input.mesh_update) { backend_input.mesh_update->timestamp_ns = timestamp_ns; - mesh_update_pub_->publish(*backend_input.mesh_update); - stored_delta_.insert( - {backend_input.mesh_update->sequence_number, backend_input.mesh_update}); - if (config.mesh_delta_queue_size > 0 && - stored_delta_.size() > static_cast(config.mesh_delta_queue_size)) { + auto delta_msg = std::make_shared(); + kimera_pgmo::conversions::to_ros(*backend_input.mesh_update, *delta_msg); + mesh_update_pub_->publish(*delta_msg); + + stored_delta_.insert({backend_input.mesh_update->info.sequence_number, delta_msg}); + if (config.mesh_delta_queue_size && + stored_delta_.size() > config.mesh_delta_queue_size) { stored_delta_.erase(stored_delta_.begin()); } } @@ -101,20 +105,19 @@ void RosFrontendPublisher::call(uint64_t timestamp_ns, dsg_sender_->sendGraph(graph, rclcpp::Time(timestamp_ns)); } -void RosFrontendPublisher::processMeshDeltaQuery( - const MeshDeltaSrv::Request::SharedPtr req, - MeshDeltaSrv::Response::SharedPtr resp) { - LOG(INFO) << "Received request for " << req->sequence_numbers.size() - << " mesh deltas..."; +void RosFrontendPublisher::processMeshDeltaQuery(const MeshDeltaRequest req, + MeshDeltaResponse resp) { + LOG(INFO) << "Received request for " << req->sequence_numbers.size() << " deltas..."; for (const auto& seq : req->sequence_numbers) { auto& msg = resp->deltas.emplace_back(); - // Check TypeAdater documentation TODO(Yun) if (!stored_delta_.count(seq)) { LOG(ERROR) << "Mesh delta sequence " << seq << " not found"; continue; } - mesh_delta_converter_.convert_to_ros_message(*stored_delta_.at(seq), msg); + + msg = *stored_delta_.at(seq); } + LOG(INFO) << "Responding with " << resp->deltas.size() << " deltas..."; } From 4356ffe7f79caf5be7ba7f9a91924cc487a15739 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Tue, 16 Dec 2025 13:09:59 -0500 Subject: [PATCH 4/5] fix missing includes from forward declares --- hydra_ros/include/hydra_ros/frontend/object_visualizer.h | 1 - hydra_ros/src/frontend/object_visualizer.cpp | 1 + hydra_ros/src/frontend/ros_frontend_publisher.cpp | 1 + 3 files changed, 2 insertions(+), 1 deletion(-) diff --git a/hydra_ros/include/hydra_ros/frontend/object_visualizer.h b/hydra_ros/include/hydra_ros/frontend/object_visualizer.h index bddf006..fc87540 100644 --- a/hydra_ros/include/hydra_ros/frontend/object_visualizer.h +++ b/hydra_ros/include/hydra_ros/frontend/object_visualizer.h @@ -35,7 +35,6 @@ #pragma once #include #include -#include #include diff --git a/hydra_ros/src/frontend/object_visualizer.cpp b/hydra_ros/src/frontend/object_visualizer.cpp index 4580025..d6c828f 100644 --- a/hydra_ros/src/frontend/object_visualizer.cpp +++ b/hydra_ros/src/frontend/object_visualizer.cpp @@ -38,6 +38,7 @@ #include #include #include +#include namespace hydra { diff --git a/hydra_ros/src/frontend/ros_frontend_publisher.cpp b/hydra_ros/src/frontend/ros_frontend_publisher.cpp index 5ae4d2e..45deb6d 100644 --- a/hydra_ros/src/frontend/ros_frontend_publisher.cpp +++ b/hydra_ros/src/frontend/ros_frontend_publisher.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include namespace hydra { From 8b9bbd8b2cebc1887d081634c64c0c24ed6cfb99 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 19 Dec 2025 10:35:01 -0500 Subject: [PATCH 5/5] add active bounding boxes for objects to rviz --- .../hydra_ros/frontend/object_visualizer.h | 11 ++-- .../launch/datasets/uhumans2.launch.yaml | 5 ++ hydra_ros/src/frontend/object_visualizer.cpp | 52 ++++++++++++++++++- 3 files changed, 61 insertions(+), 7 deletions(-) diff --git a/hydra_ros/include/hydra_ros/frontend/object_visualizer.h b/hydra_ros/include/hydra_ros/frontend/object_visualizer.h index fc87540..0145fc2 100644 --- a/hydra_ros/include/hydra_ros/frontend/object_visualizer.h +++ b/hydra_ros/include/hydra_ros/frontend/object_visualizer.h @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once #include +#include #include #include @@ -47,6 +48,8 @@ class ObjectVisualizer : public MeshSegmenter::Sink { double point_scale = 0.1; double point_alpha = 0.7; bool use_spheres = false; + double bounding_box_scale = 0.1; + visualizer::CategoricalColormap::Config colormap; } const config; explicit ObjectVisualizer(const Config& config); @@ -57,7 +60,8 @@ class ObjectVisualizer : public MeshSegmenter::Sink { void call(uint64_t timestamp_ns, const kimera_pgmo::MeshDelta& delta, - const LabelIndices& label_indices) const override; + const LabelIndices& label_indices, + const MeshSegmenter::LabelClusters& clusters) const override; protected: void fillMarkerFromCloud(const kimera_pgmo::MeshDelta& delta, @@ -68,10 +72,7 @@ class ObjectVisualizer : public MeshSegmenter::Sink { ianvs::NodeHandle nh_; ianvs::RosPublisherGroup pubs_; - private: - inline static const auto registration_ = - config::RegistrationWithConfig( - "ObjectVisualizer"); + const visualizer::CategoricalColormap colormap_; }; void declare_config(ObjectVisualizer::Config& conf); diff --git a/hydra_ros/launch/datasets/uhumans2.launch.yaml b/hydra_ros/launch/datasets/uhumans2.launch.yaml index b744bd6..3fd4185 100644 --- a/hydra_ros/launch/datasets/uhumans2.launch.yaml +++ b/hydra_ros/launch/datasets/uhumans2.launch.yaml @@ -38,3 +38,8 @@ launch: - {name: map_frame, value: map} - {name: lcd_config_path, value: $(find-pkg-share hydra)/config/lcd/uhumans2.yaml} - {name: extra_yaml, value: $(if $(var use_gt_semantics) $(var extra_yaml_gt) $(var extra_yaml_no_gt))} + - node: + pkg: tf2_ros + name: world_to_map + exec: static_transform_publisher + args: --frame-id world --child-frame-id map diff --git a/hydra_ros/src/frontend/object_visualizer.cpp b/hydra_ros/src/frontend/object_visualizer.cpp index d6c828f..126c23f 100644 --- a/hydra_ros/src/frontend/object_visualizer.cpp +++ b/hydra_ros/src/frontend/object_visualizer.cpp @@ -38,9 +38,18 @@ #include #include #include +#include #include namespace hydra { +namespace { + +static const auto registration = + config::RegistrationWithConfig("ObjectVisualizer"); + +} using visualization_msgs::msg::Marker; @@ -51,18 +60,36 @@ void declare_config(ObjectVisualizer::Config& config) { field(config.point_scale, "point_scale"); field(config.point_alpha, "point_alpha"); field(config.use_spheres, "use_spheres"); + field(config.bounding_box_scale, "bounding_box_scale"); + field(config.colormap, "colormap"); } ObjectVisualizer::ObjectVisualizer(const Config& config) : config(config::checkValid(config)), nh_(ianvs::NodeHandle::this_node(config.module_ns)), - pubs_(nh_) {} + pubs_(nh_), + colormap_(config.colormap) {} std::string ObjectVisualizer::printInfo() const { return config::toString(config); } +struct DeltaPointAdaptor : spark_dsg::BoundingBox::PointAdaptor { + DeltaPointAdaptor(const kimera_pgmo::MeshDelta& delta, + const std::vector& indices) + : delta(delta), indices(indices) {} + + size_t size() const override { return indices.size(); } + + Eigen::Vector3f get(size_t index) const override { + return delta.getVertex(indices.at(index)).pos; + } + const kimera_pgmo::MeshDelta& delta; + const std::vector& indices; +}; + void ObjectVisualizer::call(uint64_t timestamp_ns, const kimera_pgmo::MeshDelta& delta, - const LabelIndices& label_indices) const { + const LabelIndices& label_indices, + const MeshSegmenter::LabelClusters& clusters) const { pubs_.publish("active_vertices", [&]() { auto msg = std::make_unique(); msg->header.stamp = rclcpp::Time(timestamp_ns); @@ -87,6 +114,27 @@ void ObjectVisualizer::call(uint64_t timestamp_ns, return msg; }); } + + pubs_.publish("object_clusters", [&]() { + auto msg = std::make_unique(); + msg->header.stamp = rclcpp::Time(timestamp_ns); + msg->header.frame_id = GlobalInfo::instance().getFrames().odom; + msg->type = Marker::LINE_LIST; + msg->action = Marker::ADD; + msg->ns = "object_clusters"; + msg->id = 0; + msg->scale.x = config.bounding_box_scale; + for (const auto& [label, label_clusters] : clusters) { + const auto color = visualizer::makeColorMsg(colormap_(label)); + for (const auto& cluster : label_clusters) { + const DeltaPointAdaptor adaptor(delta, cluster.indices); + const spark_dsg::BoundingBox bbox(adaptor); + visualizer::drawBoundingBox(bbox, color, *msg); + } + } + + return msg; + }); } void ObjectVisualizer::fillMarkerFromCloud(const kimera_pgmo::MeshDelta& delta,