From 27bf41c6e657ca2d68908d0161a3298672a47cad Mon Sep 17 00:00:00 2001
From: Jon Binney <jon.binney@gmail.com>
Date: Tue, 27 Aug 2024 17:44:37 -0400
Subject: [PATCH 1/3] Dynamic setting of params in speckle filter

Done previously by berend-kupers, but removed during code review because
we thought it needed a separate node inside the filter. Here we use the
params_interface_ member variable of the filter base class instead of
creating a new node.

The callback needs to be post_set_params and not on_set_params because
the changes haven't been finalized yet when on_set_params is called.
---
 include/laser_filters/speckle_filter.h | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h
index f071afa..2eccfec 100644
--- a/include/laser_filters/speckle_filter.h
+++ b/include/laser_filters/speckle_filter.h
@@ -214,6 +214,9 @@ class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::msg::Lase
         break;
     }
 
+    post_set_parameters_callback_handle_ = params_interface_->add_post_set_parameters_callback(
+            std::bind(&LaserScanSpeckleFilter::reconfigureCB, this, std::placeholders::_1));
+
     return true;
   }
   /////////////////////////////////////////////////////
@@ -248,11 +251,8 @@ class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::msg::Lase
     return true;
   }
 
-  rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector<rclcpp::Parameter> parameters)
+  void reconfigureCB(std::vector<rclcpp::Parameter> parameters)
   {
-      auto result = rcl_interfaces::msg::SetParametersResult();
-      result.successful = true;
-
       for (auto parameter : parameters)
       {
         if(logging_interface_ != nullptr)
@@ -289,9 +289,6 @@ class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::msg::Lase
       default:
         break;
     }
-
-    return result;
-
   }
 
   ////////////////////////////////////////////////////
@@ -303,6 +300,7 @@ class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::msg::Lase
   double max_range = 0;
   double max_range_difference = 0;
   int filter_window = 0;
+  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_parameters_callback_handle_;
 };
 }
 #endif /* speckle_filter.h */

From ed295674b5118e8a2f61f855b6242a8d803c5258 Mon Sep 17 00:00:00 2001
From: Jon Binney <jon.binney@gmail.com>
Date: Wed, 4 Sep 2024 20:25:17 +0000
Subject: [PATCH 2/3] Disable param callback code on humble

---
 CMakeLists.txt                         | 4 ++++
 include/laser_filters/speckle_filter.h | 5 +++++
 2 files changed, 9 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e7638cd..3ab3568 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,10 @@ if(${rclcpp_VERSION_MAJOR} GREATER_EQUAL 20)
     add_compile_definitions(RCLCPP_SUPPORTS_MATCHED_CALLBACKS)
 endif()
 
+if(${rclcpp_VERSION_MAJOR} GREATER_EQUAL 17)
+    add_compile_definitions(RCLCPP_SUPPORTS_POST_SET_PARAMS_CALLBACK)
+endif()
+
 ament_auto_add_library(laser_scan_filters SHARED src/laser_scan_filters.cpp)
 ament_auto_add_library(laser_filter_chains SHARED
                          src/scan_to_cloud_filter_chain.cpp
diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h
index 2eccfec..b253bf3 100644
--- a/include/laser_filters/speckle_filter.h
+++ b/include/laser_filters/speckle_filter.h
@@ -214,8 +214,10 @@ class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::msg::Lase
         break;
     }
 
+    #ifdef RCLCPP_SUPPORTS_POST_SET_PARAMS_CALLBACK
     post_set_parameters_callback_handle_ = params_interface_->add_post_set_parameters_callback(
             std::bind(&LaserScanSpeckleFilter::reconfigureCB, this, std::placeholders::_1));
+    #endif
 
     return true;
   }
@@ -300,7 +302,10 @@ class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::msg::Lase
   double max_range = 0;
   double max_range_difference = 0;
   int filter_window = 0;
+
+  #ifdef RCLCPP_SUPPORTS_POST_SET_PARAMS_CALLBACK
   rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_parameters_callback_handle_;
+  #endif // RCLCPP_SUPPORTS_POST_SET_PARAMS_CALLBACK
 };
 }
 #endif /* speckle_filter.h */

From 027b936b167aa23cccd0edacc98a4c57d9b04bfd Mon Sep 17 00:00:00 2001
From: Jon Binney <jon.binney@gmail.com>
Date: Tue, 10 Sep 2024 20:18:49 +0000
Subject: [PATCH 3/3] Declare speckle filter params writeable

---
 include/laser_filters/speckle_filter.h | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h
index b253bf3..861f8be 100644
--- a/include/laser_filters/speckle_filter.h
+++ b/include/laser_filters/speckle_filter.h
@@ -174,6 +174,18 @@ class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::msg::Lase
 
   ///////////////////////////////////////////////////////////////
   bool configure(){
+
+    #ifdef RCLCPP_SUPPORTS_POST_SET_PARAMS_CALLBACK
+    // Declare parameters as writeable. Otherwise, the first time we call FilterBase::getParam() for each one,
+    // it will get declared as write only.
+    rcl_interfaces::msg::ParameterDescriptor desc;
+    desc.read_only = true;
+    params_interface_->declare_parameter("filter_type", rclcpp::ParameterType::PARAMETER_INTEGER, desc);
+    params_interface_->declare_parameter("max_range", rclcpp::ParameterType::PARAMETER_DOUBLE, desc);
+    params_interface_->declare_parameter("max_range_difference", rclcpp::ParameterType::PARAMETER_DOUBLE, desc);
+    params_interface_->declare_parameter("filter_window", rclcpp::ParameterType::PARAMETER_INTEGER, desc);
+    #endif
+
     // get params
     if (!filters::FilterBase<sensor_msgs::msg::LaserScan>::getParam(std::string("filter_type"), filter_type))
     {