From d0d48d80e0554bc17ac0a7c8a494754b0d731822 Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Thu, 31 Aug 2023 02:43:35 +0000
Subject: [PATCH 1/8] Expose action server default timeout in bt navigator
 servers

---
 .../bt_action_server_impl.hpp                 | 20 +++++++++++++++++--
 .../params/nav2_multirobot_params_1.yaml      |  3 +++
 .../params/nav2_multirobot_params_2.yaml      |  3 +++
 .../params/nav2_multirobot_params_all.yaml    |  3 +++
 nav2_bringup/params/nav2_params.yaml          |  3 +++
 5 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index c4f3c68a829..f8702f3c81f 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -91,6 +91,16 @@ BtActionServer<ActionT>::BtActionServer(
       RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str);
     }
   }
+
+  // Result timeout for the action server.
+  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
+  // which makes action servers discard a goal handle if the result is not produced
+  // within 10 seconds. Since this may not be the case for all actions in
+  // Nav2, this timeout is exposed as a parameter and default to the previous
+  // expiration value of 15 minutes.
+  if (!node->has_parameter("action_server_result_timeout")) {
+    node->declare_parameter("action_server_result_timeout", 15 * 60);
+  }
 }
 
 template<class ActionT>
@@ -128,15 +138,21 @@ bool BtActionServer<ActionT>::on_configure()
   client_node_->declare_parameter(
     "global_frame", node->get_parameter("global_frame").as_string());
 
+  // set the timeout in seconds for the action server to discard goal handles
+  int timeout;
+  node->get_parameter("action_server_result_timeout", timeout);
+  rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
+  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(timeout);
+
   action_server_ = std::make_shared<ActionServer>(
     node->get_node_base_interface(),
     node->get_node_clock_interface(),
     node->get_node_logging_interface(),
     node->get_node_waitables_interface(),
-    action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this));
+    action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this),
+    nullptr, std::chrono::milliseconds(500), false, server_options);
 
   // Get parameters for BT timeouts
-  int timeout;
   node->get_parameter("bt_loop_duration", timeout);
   bt_loop_duration_ = std::chrono::milliseconds(timeout);
   node->get_parameter("default_server_timeout", timeout);
diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
index c2b2dda69a3..52c7c02c291 100644
--- a/nav2_bringup/params/nav2_multirobot_params_1.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml
@@ -45,6 +45,9 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
index 54704a2e7a6..793e954cc7b 100644
--- a/nav2_bringup/params/nav2_multirobot_params_2.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml
@@ -45,6 +45,9 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900               # timeout for action servers to discard goal handles if not completed, 15 minutes previous de, see https://github.com/ros2/rcl/pull/1012
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml
index 372dcdd3f6e..267aecafb66 100644
--- a/nav2_bringup/params/nav2_multirobot_params_all.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml
@@ -45,6 +45,9 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900              # timeout for action servers to discard goal handles if not completed, 15 minutes previous de, see https://github.com/ros2/rcl/pull/1012
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index cf26d389dbd..6831d848a81 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -45,6 +45,9 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900              # timeout for action servers to discard goal handles if not completed, 15 minutes previous de, see https://github.com/ros2/rcl/pull/1012
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"

From d1049280c3afc5e634c7fb4efabbf89fa7d975e6 Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Thu, 31 Aug 2023 02:52:44 +0000
Subject: [PATCH 2/8] typo

---
 .../include/nav2_behavior_tree/bt_action_server_impl.hpp      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index f8702f3c81f..20aa02301b9 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -96,7 +96,7 @@ BtActionServer<ActionT>::BtActionServer(
   // In https://github.com/ros2/rcl/pull/1012 a change was introduced
   // which makes action servers discard a goal handle if the result is not produced
   // within 10 seconds. Since this may not be the case for all actions in
-  // Nav2, this timeout is exposed as a parameter and default to the previous
+  // Nav2, this timeout is exposed as a parameter and defaults to the previous
   // expiration value of 15 minutes.
   if (!node->has_parameter("action_server_result_timeout")) {
     node->declare_parameter("action_server_result_timeout", 15 * 60);
@@ -138,7 +138,7 @@ bool BtActionServer<ActionT>::on_configure()
   client_node_->declare_parameter(
     "global_frame", node->get_parameter("global_frame").as_string());
 
-  // set the timeout in seconds for the action server to discard goal handles
+  // set the timeout in seconds for the action server to discard goal handles if not finished
   int timeout;
   node->get_parameter("action_server_result_timeout", timeout);
   rcl_action_server_options_t server_options = rcl_action_server_get_default_options();

From b1afe3470b0620e15012bbf8971804456ac6a764 Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Thu, 31 Aug 2023 02:54:29 +0000
Subject: [PATCH 3/8] duplicated comment

---
 nav2_bringup/params/nav2_multirobot_params_2.yaml   | 2 +-
 nav2_bringup/params/nav2_multirobot_params_all.yaml | 2 +-
 nav2_bringup/params/nav2_params.yaml                | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
index 793e954cc7b..2e8ebe96649 100644
--- a/nav2_bringup/params/nav2_multirobot_params_2.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml
@@ -47,7 +47,7 @@ bt_navigator:
     default_server_timeout: 20
     # timeout [s] for action servers to discard goal handles if not completed, 
     # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
-    action_server_result_timeout: 900               # timeout for action servers to discard goal handles if not completed, 15 minutes previous de, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml
index 267aecafb66..0932a4592b9 100644
--- a/nav2_bringup/params/nav2_multirobot_params_all.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml
@@ -47,7 +47,7 @@ bt_navigator:
     default_server_timeout: 20
     # timeout [s] for action servers to discard goal handles if not completed, 
     # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
-    action_server_result_timeout: 900              # timeout for action servers to discard goal handles if not completed, 15 minutes previous de, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 6831d848a81..703dfd339fd 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -47,7 +47,7 @@ bt_navigator:
     default_server_timeout: 20
     # timeout [s] for action servers to discard goal handles if not completed, 
     # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
-    action_server_result_timeout: 900              # timeout for action servers to discard goal handles if not completed, 15 minutes previous de, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"

From 12763d777c2c319578850f52f5884be9db531fbd Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Sat, 2 Sep 2023 19:34:54 +0000
Subject: [PATCH 4/8] Expose result timeout in other actions

---
 .../bt_action_server_impl.hpp                 | 16 +++++++++-------
 .../include/nav2_behaviors/timed_behavior.hpp | 18 +++++++++++++++++-
 .../params/nav2_multirobot_params_1.yaml      |  5 ++++-
 .../params/nav2_multirobot_params_2.yaml      |  5 ++++-
 .../params/nav2_multirobot_params_all.yaml    |  5 ++++-
 nav2_bringup/params/nav2_params.yaml          |  5 ++++-
 nav2_controller/src/controller_server.cpp     | 15 ++++++++++++++-
 nav2_planner/src/planner_server.cpp           | 17 +++++++++++++++--
 nav2_smoother/src/nav2_smoother.cpp           | 15 ++++++++++++++-
 .../src/waypoint_follower.cpp                 | 19 ++++++++++++++++++-
 10 files changed, 103 insertions(+), 17 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index 20aa02301b9..92e88b9aabc 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -99,7 +99,7 @@ BtActionServer<ActionT>::BtActionServer(
   // Nav2, this timeout is exposed as a parameter and defaults to the previous
   // expiration value of 15 minutes.
   if (!node->has_parameter("action_server_result_timeout")) {
-    node->declare_parameter("action_server_result_timeout", 15 * 60);
+    node->declare_parameter("action_server_result_timeout", 900.0);
   }
 }
 
@@ -139,10 +139,10 @@ bool BtActionServer<ActionT>::on_configure()
     "global_frame", node->get_parameter("global_frame").as_string());
 
   // set the timeout in seconds for the action server to discard goal handles if not finished
-  int timeout;
-  node->get_parameter("action_server_result_timeout", timeout);
+  double action_server_result_timeout;
+  node->get_parameter("action_server_result_timeout", action_server_result_timeout);
   rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
-  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(timeout);
+  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
 
   action_server_ = std::make_shared<ActionServer>(
     node->get_node_base_interface(),
@@ -153,10 +153,12 @@ bool BtActionServer<ActionT>::on_configure()
     nullptr, std::chrono::milliseconds(500), false, server_options);
 
   // Get parameters for BT timeouts
-  node->get_parameter("bt_loop_duration", timeout);
+  int bt_loop_duration;
+  node->get_parameter("bt_loop_duration", bt_loop_duration);
   bt_loop_duration_ = std::chrono::milliseconds(timeout);
-  node->get_parameter("default_server_timeout", timeout);
-  default_server_timeout_ = std::chrono::milliseconds(timeout);
+  int default_server_timeout;
+  node->get_parameter("default_server_timeout", default_server_timeout);
+  default_server_timeout_ = std::chrono::milliseconds(default_server_timeout);
 
   // Get error code id names to grab off of the blackboard
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
index 821818dd2f2..4c39b6de49f 100644
--- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -133,9 +133,25 @@ class TimedBehavior : public nav2_core::Behavior
     node->get_parameter("robot_base_frame", robot_base_frame_);
     node->get_parameter("transform_tolerance", transform_tolerance_);
 
+    // Result timeout for the action server.
+    // In https://github.com/ros2/rcl/pull/1012 a change was introduced
+    // which makes action servers discard a goal handle if the result is not produced
+    // within 10 seconds. Since this may not be the case for all actions in
+    // Nav2, this timeout is exposed as a parameter and defaults to the previous
+    // expiration value of 15 minutes.
+    if (!node->has_parameter("action_server_result_timeout")) {
+      node->declare_parameter("action_server_result_timeout", 900.0);
+    }
+
+    double action_server_result_timeout;
+    node->get_parameter("action_server_result_timeout", action_server_result_timeout);
+    rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
+    server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
+
     action_server_ = std::make_shared<ActionServer>(
       node, behavior_name_,
-      std::bind(&TimedBehavior::execute, this));
+      std::bind(&TimedBehavior::execute, this), nullptr, std::chrono::milliseconds(
+        500), false, server_options);
 
     local_collision_checker_ = local_collision_checker;
     global_collision_checker_ = global_collision_checker;
diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
index 52c7c02c291..7e96abe32dd 100644
--- a/nav2_bringup/params/nav2_multirobot_params_1.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml
@@ -47,7 +47,7 @@ bt_navigator:
     default_server_timeout: 20
     # timeout [s] for action servers to discard goal handles if not completed, 
     # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
-    action_server_result_timeout: 900
+    action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
@@ -280,6 +280,9 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
index 2e8ebe96649..11e9d2a9af7 100644
--- a/nav2_bringup/params/nav2_multirobot_params_2.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml
@@ -47,7 +47,7 @@ bt_navigator:
     default_server_timeout: 20
     # timeout [s] for action servers to discard goal handles if not completed, 
     # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
-    action_server_result_timeout: 900
+    action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
@@ -279,6 +279,9 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml
index 0932a4592b9..b2ff9a40df0 100644
--- a/nav2_bringup/params/nav2_multirobot_params_all.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml
@@ -47,7 +47,7 @@ bt_navigator:
     default_server_timeout: 20
     # timeout [s] for action servers to discard goal handles if not completed, 
     # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
-    action_server_result_timeout: 900
+    action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
@@ -327,6 +327,9 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 703dfd339fd..6873c101fbc 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -47,7 +47,7 @@ bt_navigator:
     default_server_timeout: 20
     # timeout [s] for action servers to discard goal handles if not completed, 
     # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
-    action_server_result_timeout: 900
+    action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
@@ -323,6 +323,9 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    # timeout [s] for action servers to discard goal handles if not completed, 
+    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index c76f9f86ccd..93418124f68 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -50,6 +50,14 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 
   declare_parameter("controller_frequency", 20.0);
 
+  // Result timeout for the action server.
+  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
+  // which makes action servers discard a goal handle if the result is not produced
+  // within 10 seconds. Since this may not be the case for all actions in
+  // Nav2, this timeout is exposed as a parameter and defaults to the previous
+  // expiration value of 15 minutes.
+  declare_parameter("action_server_result_timeout", 900.0);
+
   declare_parameter("progress_checker_plugins", default_progress_checker_ids_);
   declare_parameter("goal_checker_plugins", default_goal_checker_ids_);
   declare_parameter("controller_plugins", default_ids_);
@@ -227,6 +235,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
   vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
 
+  double action_server_result_timeout;
+  get_parameter("action_server_result_timeout", action_server_result_timeout);
+  rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
+  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
+
   // Create the action server that we implement with our followPath method
   action_server_ = std::make_unique<ActionServer>(
     shared_from_this(),
@@ -234,7 +247,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     std::bind(&ControllerServer::computeControl, this),
     nullptr,
     std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   // Set subscribtion to the speed limiting topic
   speed_limit_sub_ = create_subscription<nav2_msgs::msg::SpeedLimit>(
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index f1e3cd9c5d0..479b25254ab 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -53,6 +53,14 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   declare_parameter("planner_plugins", default_ids_);
   declare_parameter("expected_planner_frequency", 1.0);
 
+  // Result timeout for the action server.
+  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
+  // which makes action servers discard a goal handle if the result is not produced
+  // within 10 seconds. Since this may not be the case for all actions in
+  // Nav2, this timeout is exposed as a parameter and defaults to the previous
+  // expiration value of 15 minutes.
+  declare_parameter("action_server_result_timeout", 900.0);
+
   get_parameter("planner_plugins", planner_ids_);
   if (planner_ids_ == default_ids_) {
     for (size_t i = 0; i < default_ids_.size(); ++i) {
@@ -139,6 +147,11 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Initialize pubs & subs
   plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1);
 
+  double action_server_result_timeout;
+  get_parameter("action_server_result_timeout", action_server_result_timeout);
+  rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
+  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
+
   // Create the action servers for path planning to a pose and through poses
   action_server_pose_ = std::make_unique<ActionServerToPose>(
     shared_from_this(),
@@ -146,7 +159,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     std::bind(&PlannerServer::computePlan, this),
     nullptr,
     std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   action_server_poses_ = std::make_unique<ActionServerThroughPoses>(
     shared_from_this(),
@@ -154,7 +167,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     std::bind(&PlannerServer::computePlanThroughPoses, this),
     nullptr,
     std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
index 522d3618304..c98a84e1214 100644
--- a/nav2_smoother/src/nav2_smoother.cpp
+++ b/nav2_smoother/src/nav2_smoother.cpp
@@ -53,6 +53,14 @@ SmootherServer::SmootherServer(const rclcpp::NodeOptions & options)
     rclcpp::ParameterValue(std::string("base_link")));
   declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1));
   declare_parameter("smoother_plugins", default_ids_);
+
+  // Result timeout for the action server.
+  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
+  // which makes action servers discard a goal handle if the result is not produced
+  // within 10 seconds. Since this may not be the case for all actions in
+  // Nav2, this timeout is exposed as a parameter and defaults to the previous
+  // expiration value of 15 minutes.
+  declare_parameter("action_server_result_timeout", 900.0);
 }
 
 SmootherServer::~SmootherServer()
@@ -104,6 +112,11 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &)
   // Initialize pubs & subs
   plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan_smoothed", 1);
 
+  double action_server_result_timeout;
+  get_parameter("action_server_result_timeout", action_server_result_timeout);
+  rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
+  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
+
   // Create the action server that we implement with our smoothPath method
   action_server_ = std::make_unique<ActionServer>(
     shared_from_this(),
@@ -111,7 +124,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &)
     std::bind(&SmootherServer::smoothPlan, this),
     nullptr,
     std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
index fb214bd41c1..4d97e7c8f52 100644
--- a/nav2_waypoint_follower/src/waypoint_follower.cpp
+++ b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -36,6 +36,15 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options)
 
   declare_parameter("stop_on_failure", true);
   declare_parameter("loop_rate", 20);
+
+  // Result timeout for the action server.
+  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
+  // which makes action servers discard a goal handle if the result is not produced
+  // within 10 seconds. Since this may not be the case for all actions in
+  // Nav2, this timeout is exposed as a parameter and defaults to the previous
+  // expiration value of 15 minutes.
+  declare_parameter("action_server_result_timeout", 900.0);
+
   nav2_util::declare_parameter_if_not_declared(
     this, std::string("waypoint_task_executor_plugin"),
     rclcpp::ParameterValue(std::string("wait_at_waypoint")));
@@ -71,12 +80,20 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/)
     get_node_waitables_interface(),
     "navigate_to_pose", callback_group_);
 
+  double action_server_result_timeout;
+  get_parameter("action_server_result_timeout", action_server_result_timeout);
+  rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
+  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
+
   action_server_ = std::make_unique<ActionServer>(
     get_node_base_interface(),
     get_node_clock_interface(),
     get_node_logging_interface(),
     get_node_waitables_interface(),
-    "follow_waypoints", std::bind(&WaypointFollower::followWaypoints, this));
+    "follow_waypoints", std::bind(
+      &WaypointFollower::followWaypoints,
+      this), nullptr, std::chrono::milliseconds(
+      500), false, server_options);
 
   try {
     waypoint_task_executor_type_ = nav2_util::get_plugin_type_param(

From f853f5243825f753670ae73b99c3b424a74d5319 Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Sat, 2 Sep 2023 19:48:50 +0000
Subject: [PATCH 5/8] Proper timeout in bt node

---
 .../include/nav2_behavior_tree/bt_action_server_impl.hpp        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index ba3fea961a4..d75251ff195 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -161,7 +161,7 @@ bool BtActionServer<ActionT>::on_configure()
   // Get parameters for BT timeouts
   int bt_loop_duration;
   node->get_parameter("bt_loop_duration", bt_loop_duration);
-  bt_loop_duration_ = std::chrono::milliseconds(timeout);
+  bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration);
   int default_server_timeout;
   node->get_parameter("default_server_timeout", default_server_timeout);
   default_server_timeout_ = std::chrono::milliseconds(default_server_timeout);

From 7156a5f9260b0597d8647836a4e4fd0dea62802f Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Thu, 7 Sep 2023 01:07:01 +0000
Subject: [PATCH 6/8] Change default timeouts and remove comments

---
 .../nav2_behavior_tree/bt_action_server_impl.hpp    | 13 +++----------
 .../include/nav2_behaviors/timed_behavior.hpp       |  8 +-------
 nav2_bringup/params/nav2_multirobot_params_2.yaml   |  4 ----
 nav2_bringup/params/nav2_multirobot_params_all.yaml |  4 ----
 nav2_bringup/params/nav2_params.yaml                |  4 ----
 nav2_controller/src/controller_server.cpp           | 10 ++--------
 nav2_planner/src/planner_server.cpp                 |  8 +-------
 nav2_smoother/src/nav2_smoother.cpp                 |  8 +-------
 nav2_waypoint_follower/src/waypoint_follower.cpp    |  6 ------
 9 files changed, 8 insertions(+), 57 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index d75251ff195..50b8d8ac526 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -61,6 +61,9 @@ BtActionServer<ActionT>::BtActionServer(
   if (!node->has_parameter("default_server_timeout")) {
     node->declare_parameter("default_server_timeout", 20);
   }
+  if (!node->has_parameter("action_server_result_timeout")) {
+    node->declare_parameter("action_server_result_timeout", 900.0);
+  }
 
   std::vector<std::string> error_code_names = {
     "follow_path_error_code",
@@ -92,16 +95,6 @@ BtActionServer<ActionT>::BtActionServer(
       RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str);
     }
   }
-
-  // Result timeout for the action server.
-  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
-  // which makes action servers discard a goal handle if the result is not produced
-  // within 10 seconds. Since this may not be the case for all actions in
-  // Nav2, this timeout is exposed as a parameter and defaults to the previous
-  // expiration value of 15 minutes.
-  if (!node->has_parameter("action_server_result_timeout")) {
-    node->declare_parameter("action_server_result_timeout", 900.0);
-  }
 }
 
 template<class ActionT>
diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
index 4c39b6de49f..6af6490b618 100644
--- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -133,14 +133,8 @@ class TimedBehavior : public nav2_core::Behavior
     node->get_parameter("robot_base_frame", robot_base_frame_);
     node->get_parameter("transform_tolerance", transform_tolerance_);
 
-    // Result timeout for the action server.
-    // In https://github.com/ros2/rcl/pull/1012 a change was introduced
-    // which makes action servers discard a goal handle if the result is not produced
-    // within 10 seconds. Since this may not be the case for all actions in
-    // Nav2, this timeout is exposed as a parameter and defaults to the previous
-    // expiration value of 15 minutes.
     if (!node->has_parameter("action_server_result_timeout")) {
-      node->declare_parameter("action_server_result_timeout", 900.0);
+      node->declare_parameter("action_server_result_timeout", 10.0);
     }
 
     double action_server_result_timeout;
diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
index 11e9d2a9af7..2d57aef3830 100644
--- a/nav2_bringup/params/nav2_multirobot_params_2.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml
@@ -45,8 +45,6 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
@@ -279,8 +277,6 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml
index b2ff9a40df0..6f7e66cd617 100644
--- a/nav2_bringup/params/nav2_multirobot_params_all.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml
@@ -45,8 +45,6 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
@@ -327,8 +325,6 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 6873c101fbc..112a1de98b1 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -45,8 +45,6 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
@@ -323,8 +321,6 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index 93418124f68..9e668b70458 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -49,14 +49,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   RCLCPP_INFO(get_logger(), "Creating controller server");
 
   declare_parameter("controller_frequency", 20.0);
-
-  // Result timeout for the action server.
-  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
-  // which makes action servers discard a goal handle if the result is not produced
-  // within 10 seconds. Since this may not be the case for all actions in
-  // Nav2, this timeout is exposed as a parameter and defaults to the previous
-  // expiration value of 15 minutes.
-  declare_parameter("action_server_result_timeout", 900.0);
+  
+  declare_parameter("action_server_result_timeout", 10.0);
 
   declare_parameter("progress_checker_plugins", default_progress_checker_ids_);
   declare_parameter("goal_checker_plugins", default_goal_checker_ids_);
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 479b25254ab..2add3d91fdc 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -53,13 +53,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   declare_parameter("planner_plugins", default_ids_);
   declare_parameter("expected_planner_frequency", 1.0);
 
-  // Result timeout for the action server.
-  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
-  // which makes action servers discard a goal handle if the result is not produced
-  // within 10 seconds. Since this may not be the case for all actions in
-  // Nav2, this timeout is exposed as a parameter and defaults to the previous
-  // expiration value of 15 minutes.
-  declare_parameter("action_server_result_timeout", 900.0);
+  declare_parameter("action_server_result_timeout", 10.0);
 
   get_parameter("planner_plugins", planner_ids_);
   if (planner_ids_ == default_ids_) {
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
index c98a84e1214..6545d3e724f 100644
--- a/nav2_smoother/src/nav2_smoother.cpp
+++ b/nav2_smoother/src/nav2_smoother.cpp
@@ -54,13 +54,7 @@ SmootherServer::SmootherServer(const rclcpp::NodeOptions & options)
   declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1));
   declare_parameter("smoother_plugins", default_ids_);
 
-  // Result timeout for the action server.
-  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
-  // which makes action servers discard a goal handle if the result is not produced
-  // within 10 seconds. Since this may not be the case for all actions in
-  // Nav2, this timeout is exposed as a parameter and defaults to the previous
-  // expiration value of 15 minutes.
-  declare_parameter("action_server_result_timeout", 900.0);
+  declare_parameter("action_server_result_timeout", 10.0);
 }
 
 SmootherServer::~SmootherServer()
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
index 4d97e7c8f52..4e051d599b3 100644
--- a/nav2_waypoint_follower/src/waypoint_follower.cpp
+++ b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -37,12 +37,6 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options)
   declare_parameter("stop_on_failure", true);
   declare_parameter("loop_rate", 20);
 
-  // Result timeout for the action server.
-  // In https://github.com/ros2/rcl/pull/1012 a change was introduced
-  // which makes action servers discard a goal handle if the result is not produced
-  // within 10 seconds. Since this may not be the case for all actions in
-  // Nav2, this timeout is exposed as a parameter and defaults to the previous
-  // expiration value of 15 minutes.
   declare_parameter("action_server_result_timeout", 900.0);
 
   nav2_util::declare_parameter_if_not_declared(

From 8ea42a128173dc0cc54b3db8bab4c190542b9bc3 Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Thu, 7 Sep 2023 01:09:00 +0000
Subject: [PATCH 7/8] Remove comment in params file

---
 nav2_bringup/params/nav2_multirobot_params_1.yaml | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
index 7e96abe32dd..692ca6ec803 100644
--- a/nav2_bringup/params/nav2_multirobot_params_1.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml
@@ -45,8 +45,6 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
@@ -280,8 +278,6 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    # timeout [s] for action servers to discard goal handles if not completed, 
-    # 15 minutes previous default, see https://github.com/ros2/rcl/pull/1012
     action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:

From cb6270bef2d9c7c33376d9e6588c2b26425f88b7 Mon Sep 17 00:00:00 2001
From: pepisg <pedro.gonzalez@eia.edu.co>
Date: Sat, 9 Sep 2023 14:01:58 +0000
Subject: [PATCH 8/8] uncrustify controller server

---
 nav2_controller/src/controller_server.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index 9e668b70458..241ccebc243 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -49,7 +49,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   RCLCPP_INFO(get_logger(), "Creating controller server");
 
   declare_parameter("controller_frequency", 20.0);
-  
+
   declare_parameter("action_server_result_timeout", 10.0);
 
   declare_parameter("progress_checker_plugins", default_progress_checker_ids_);