From 1432d448d5c23a4635e783f543ab899cbb2c1045 Mon Sep 17 00:00:00 2001
From: "G.Doisy" <doisyg@users.noreply.github.com>
Date: Thu, 25 Feb 2021 20:58:50 +0100
Subject: [PATCH] [NavFn] Make the 3 parameters changeable at runtime (#2181)

* make the 3 params changeable at runtime

* use parameter events callbacks

* doxygen

* lint
---
 .../nav2_navfn_planner/navfn_planner.hpp      | 10 ++++++
 nav2_navfn_planner/src/navfn_planner.cpp      | 35 +++++++++++++++++++
 2 files changed, 45 insertions(+)

diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
index ec734112aa..66f57790b7 100644
--- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
+++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
@@ -210,6 +210,16 @@ class NavfnPlanner : public nav2_core::GlobalPlanner
 
   // Whether to use the astar planner or default dijkstras
   bool use_astar_;
+
+  // Subscription for parameter change
+  rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
+  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
+
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param event ParameterEvent message
+   */
+  void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
 };
 
 }  // namespace nav2_navfn_planner
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index a2b412b776..f9fffe25ea 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -40,6 +40,8 @@
 
 using namespace std::chrono_literals;
 using nav2_util::declare_parameter_if_not_declared;
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
 
 namespace nav2_navfn_planner
 {
@@ -85,6 +87,16 @@ NavfnPlanner::configure(
   planner_ = std::make_unique<NavFn>(
     costmap_->getSizeInCellsX(),
     costmap_->getSizeInCellsY());
+
+  // Setup callback for changes to parameters.
+  parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+
+  parameter_event_sub_ = parameters_client_->on_parameter_event(
+    std::bind(&NavfnPlanner::on_parameter_event_callback, this, _1));
 }
 
 void
@@ -453,6 +465,29 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my)
   costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE);
 }
 
+void
+NavfnPlanner::on_parameter_event_callback(
+  const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+{
+  for (auto & changed_parameter : event->changed_parameters) {
+    const auto & type = changed_parameter.value.type;
+    const auto & name = changed_parameter.name;
+    const auto & value = changed_parameter.value;
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == name_ + ".tolerance") {
+        tolerance_ = value.double_value;
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == name_ + ".use_astar") {
+        use_astar_ = value.bool_value;
+      } else if (name == name_ + ".allow_unknown") {
+        allow_unknown_ = value.bool_value;
+      }
+    }
+  }
+}
+
 }  // namespace nav2_navfn_planner
 
 #include "pluginlib/class_list_macros.hpp"