From aba99dfd0ff8c123509c08d4ee37006de1d5ba37 Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <guillaume.doisy@wyca.fr>
Date: Wed, 6 Jul 2022 00:31:22 +0200
Subject: [PATCH] nav2 PR 2489 adaptation (NOT TESTED)

---
 .../spatio_temporal_voxel_layer.hpp           | 15 ++++---
 src/spatio_temporal_voxel_layer.cpp           | 41 ++++++++++++-------
 2 files changed, 36 insertions(+), 20 deletions(-)

diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
index 4a56787e..84a23e65 100644
--- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
+++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
@@ -80,7 +80,7 @@ namespace spatio_temporal_voxel_layer
 {
 
 // conveniences for line lengths
-typedef std::vector<std::shared_ptr<message_filters::SubscriberBase<>>>::iterator
+typedef std::vector<std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>>>::iterator
   observation_subscribers_iter;
 typedef std::vector<std::shared_ptr<buffer::MeasurementBuffer>>::iterator observation_buffers_iter;
 
@@ -148,12 +148,13 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
   bool RemoveStaticObservations(void);
 
   // Enable/Disable callback
-  void BufferEnablerCallback(
-    const std::shared_ptr<rmw_request_id_t> request_header,
+  void BufferEnablerCallback(const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
     std::shared_ptr<std_srvs::srv::SetBool::Response> response,
     const std::shared_ptr<buffer::MeasurementBuffer> buffer,
-    const std::shared_ptr<message_filters::SubscriberBase<>> & subcriber);
+    const std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>>
+      &subcriber
+    );
 
   /**
    * @brief Callback executed when a paramter change is detected
@@ -163,7 +164,11 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
     dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
   laser_geometry::LaserProjection _laser_projector;
-  std::vector<std::shared_ptr<message_filters::SubscriberBase<>>> _observation_subscribers;
+
+  //message_filters::Subscriber<sensor_msgs::msg::LaserScan, rclcpp_lifecycle::LifecycleNode>
+
+  std::vector<std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>>>
+    _observation_subscribers;
   std::vector<std::shared_ptr<tf2_ros::MessageFilterBase>> _observation_notifiers;
   std::vector<std::shared_ptr<buffer::MeasurementBuffer>> _observation_buffers;
   std::vector<std::shared_ptr<buffer::MeasurementBuffer>> _marking_buffers;
diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp
index 04cc7905..7f353b9c 100644
--- a/src/spatio_temporal_voxel_layer.cpp
+++ b/src/spatio_temporal_voxel_layer.cpp
@@ -140,13 +140,19 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     default_value_ = nav2_costmap_2d::FREE_SPACE;
   }
 
-  _voxel_pub = rclcpp_node_->create_publisher<sensor_msgs::msg::PointCloud2>(
-    "voxel_grid", rclcpp::QoS(1));
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = callback_group_;
+
+  auto pub_opt = rclcpp::PublisherOptions();
+  sub_opt.callback_group = callback_group_;
+
+  _voxel_pub = node->create_publisher<sensor_msgs::msg::PointCloud2>(
+    "voxel_grid", rclcpp::QoS(1), pub_opt);
 
   auto save_grid_callback = std::bind(
     &SpatioTemporalVoxelLayer::SaveGridCallback, this, _1, _2, _3);
-  _grid_saver = rclcpp_node_->create_service<spatio_temporal_voxel_layer::srv::SaveGrid>(
-    "save_grid", save_grid_callback);
+  _grid_saver = node->create_service<spatio_temporal_voxel_layer::srv::SaveGrid>(
+    "save_grid", save_grid_callback, rmw_qos_profile_services_default, callback_group_);
 
   _voxel_grid = std::make_unique<volume_grid::SpatioTemporalVoxelGrid>(
     node->get_clock(), _voxel_size, static_cast<double>(default_value_), _decay_model,
@@ -279,13 +285,15 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
 
     // create a callback for the topic
     if (data_type == "LaserScan") {
-      std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>
-      > sub(new message_filters::Subscriber<sensor_msgs::msg::LaserScan>(
-          rclcpp_node_, topic, custom_qos_profile));
+      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
 
       std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>
       > filter(new tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>(
-          *sub, *tf_, _global_frame, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance)));
+          *sub, *tf_, _global_frame, 50,
+                 node->get_node_logging_interface(),
+                 node->get_node_clock_interface(),
+                 tf2::durationFromSec(transform_tolerance)));
 
       if (inf_is_valid) {
         filter->registerCallback(
@@ -304,13 +312,15 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
 
       _observation_notifiers.back()->setTolerance(rclcpp::Duration::from_seconds(0.05));
     } else if (data_type == "PointCloud2") {
-      std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>
-      > sub(new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(
-          rclcpp_node_, topic, custom_qos_profile));
+      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
+          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
 
       std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>
       > filter(new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(
-          *sub, *tf_, _global_frame, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance)));
+          *sub, *tf_, _global_frame, 50,
+                 node->get_node_logging_interface(),
+                 node->get_node_clock_interface(),
+                 tf2::durationFromSec(transform_tolerance)));
       filter->registerCallback(
         std::bind(
           &SpatioTemporalVoxelLayer::PointCloud2Callback, this, _1,
@@ -329,8 +339,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
       _1, _2, _3, _observation_buffers.back(),
       _observation_subscribers.back());
     std::string toggle_topic = source + "/toggle_enabled";
-    auto server = rclcpp_node_->create_service<std_srvs::srv::SetBool>(
-      toggle_topic, toggle_srv_callback);
+    auto server = node->create_service<std_srvs::srv::SetBool>(
+      toggle_topic, toggle_srv_callback, rmw_qos_profile_services_default, callback_group_);
 
     _buffer_enabler_servers.push_back(server);
 
@@ -425,7 +435,8 @@ void SpatioTemporalVoxelLayer::BufferEnablerCallback(
   const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
   std::shared_ptr<std_srvs::srv::SetBool::Response> response,
   const std::shared_ptr<buffer::MeasurementBuffer> buffer,
-  const std::shared_ptr<message_filters::SubscriberBase<>> & subcriber)
+  const std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>> &subcriber
+  )
 /*****************************************************************************/
 {
   buffer->Lock();