fix frontend delta publishing

Author: nathanhhughes

hydra_ros/include/hydra_ros/frontend/ros_frontend_publisher.h

@@ -34,12 +34,11 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 #include <hydra/frontend/graph_builder.h>
-#include <kimera_pgmo_ros/conversion/mesh_delta.h>
 #include <pose_graph_tools_ros/conversions.h>
 
 #include <map>
-#include <queue>
 
+#include <kimera_pgmo_msgs/msg/mesh_delta.hpp>
 #include <kimera_pgmo_msgs/srv/mesh_delta_query.hpp>
 
 #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<DsgSender> dsg_sender_;
-  mutable std::map<uint16_t, kimera_pgmo::MeshDelta::Ptr> stored_delta_;
-
   pose_graph_tools::PoseGraphPublisher mesh_graph_pub_;
-  kimera_pgmo::PgmoMeshDeltaPublisher mesh_update_pub_;
+  rclcpp::Publisher<MeshDeltaMsg>::SharedPtr mesh_update_pub_;
   rclcpp::Service<MeshDeltaSrv>::SharedPtr mesh_delta_server_;
-  rclcpp::TypeAdapter<kimera_pgmo::MeshDelta, kimera_pgmo_msgs::msg::MeshDelta>
-      mesh_delta_converter_;
+
+  mutable std::map<uint16_t, MeshDeltaMsg::SharedPtr> stored_delta_;
 };
+
 }  // namespace hydra

hydra_ros/src/frontend/ros_frontend_publisher.cpp

@@ -39,10 +39,10 @@
 #include <config_utilities/printing.h>
 #include <config_utilities/validation.h>
 #include <hydra/common/global_info.h>
+#include <kimera_pgmo_ros/conversion/mesh_delta.h>
 
 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<BaseInterface>()->create_callback_group(
       CallbackGroupType::MutuallyExclusive);
   mesh_delta_server_ =
@@ -75,46 +76,48 @@ RosFrontendPublisher::RosFrontendPublisher(ianvs::NodeHandle nh)
                                       this,
                                       rclcpp::ServicesQoS(),
                                       group);
-  dsg_sender_ = std::make_unique<DsgSender>(config.dsg_sender, nh);
-  mesh_graph_pub_ = nh.create_publisher<PoseGraphTypeAdapter>(
-      "mesh_graph_incremental", rclcpp::QoS(100).transient_local());
-  mesh_update_pub_ = nh.create_publisher<MeshDeltaTypeAdapter>(
-      "full_mesh_update", rclcpp::QoS(100).transient_local());
+
+  const auto qos = rclcpp::QoS(100).transient_local();
+  mesh_graph_pub_ =
+      nh.create_publisher<PoseGraphTypeAdapter>("mesh_graph_incremental", qos);
+  mesh_update_pub_ = nh.create_publisher<MeshDeltaMsg>("full_mesh_update", qos);
 }
 
 void RosFrontendPublisher::call(uint64_t timestamp_ns,
                                 const DynamicSceneGraph& graph,
                                 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<size_t>(config.mesh_delta_queue_size)) {
+    auto delta_msg = std::make_shared<MeshDeltaMsg>();
+    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());
     }
   }
 
   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...";
 }