Feature/presistent deltas (#21)

* track deltas and add request service

* fix callback

* add success field

* minor tweaks

* continue if deltas do not exist

* update service name

* address nathan's comments

Author: yunzc

hydra_ros/include/hydra_ros/frontend/ros_frontend_publisher.h

@@ -37,15 +37,23 @@
 #include <kimera_pgmo_ros/conversion/mesh_delta.h>
 #include <pose_graph_tools_ros/conversions.h>
 
+#include <map>
+#include <queue>
+
+#include <kimera_pgmo_msgs/srv/mesh_delta_query.hpp>
+
 #include "hydra_ros/utils/dsg_streaming_interface.h"
 
 namespace hydra {
 
 class RosFrontendPublisher : public GraphBuilder::Sink {
  public:
+  using MeshDeltaSrv = kimera_pgmo_msgs::srv::MeshDeltaQuery;
+
   struct Config {
     //! @brief Configuration for dsg publisher
     DsgSender::Config dsg_sender;
+    size_t mesh_delta_queue_size = 100;  // Store mesh delta to resend. 0 for infinite
   } const config;
 
   explicit RosFrontendPublisher(ianvs::NodeHandle);
@@ -57,9 +65,16 @@ class RosFrontendPublisher : public GraphBuilder::Sink {
   std::string printInfo() const override { return "RosFrontendPublisher"; }
 
  protected:
+  void processMeshDeltaQuery(const MeshDeltaSrv::Request::SharedPtr req,
+                             MeshDeltaSrv::Response::SharedPtr 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::Service<MeshDeltaSrv>::SharedPtr mesh_delta_server_;
+  rclcpp::TypeAdapter<kimera_pgmo::MeshDelta, kimera_pgmo_msgs::msg::MeshDelta>
+      mesh_delta_converter_;
 };
-
 }  // namespace hydra

hydra_ros/src/frontend/ros_frontend_publisher.cpp

@@ -44,6 +44,8 @@ namespace hydra {
 
 using kimera_pgmo::MeshDeltaTypeAdapter;
 using pose_graph_tools::PoseGraphTypeAdapter;
+using BaseInterface = rclcpp::node_interfaces::NodeBaseInterface;
+using rclcpp::CallbackGroupType;
 
 namespace {
 
@@ -60,10 +62,19 @@ void declare_config(RosFrontendPublisher::Config& config) {
   using namespace config;
   name("RosFrontendPublisher::Config");
   field(config.dsg_sender, "");
+  field(config.mesh_delta_queue_size, "mesh_delta_queue_size");
 }
 
 RosFrontendPublisher::RosFrontendPublisher(ianvs::NodeHandle nh)
     : config(config::checkValid(get_config())) {
+  auto group = nh.as<BaseInterface>()->create_callback_group(
+      CallbackGroupType::MutuallyExclusive);
+  mesh_delta_server_ =
+      nh.create_service<MeshDeltaSrv>("mesh_delta_query",
+                                      &RosFrontendPublisher::processMeshDeltaQuery,
+                                      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());
@@ -79,9 +90,32 @@ void RosFrontendPublisher::call(uint64_t timestamp_ns,
   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)) {
+      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...";
+  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);
+  }
+  LOG(INFO) << "Responding with " << resp->deltas.size() << " deltas...";
+}
+
 }  // namespace hydra