[NavFn] Make the 3 parameters changeable at runtime

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -213,6 +213,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

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
 {
@@ -88,6 +90,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
@@ -459,6 +471,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"