Revert "Removing action server timeout duration after fixes to ROS 2, Reverts 3787 (#5183)"

This reverts commit c9438b4.

Author: SteveMacenski

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -66,6 +66,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);
+  }
   if (!node->has_parameter("always_reload_bt_xml")) {
     node->declare_parameter("always_reload_bt_xml", false);
   }
@@ -164,13 +167,19 @@ bool BtActionServer<ActionT>::on_configure()
     node, "transform_tolerance", rclcpp::ParameterValue(0.1));
   nav2_util::copy_all_parameter_values(node, client_node_);
 
+  // set the timeout in seconds for the action server to discard goal handles if not finished
+  double action_server_result_timeout =
+    node->get_parameter("action_server_result_timeout").as_double();
+  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->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),
-    nullptr, std::chrono::milliseconds(500), false);
+    nullptr, std::chrono::milliseconds(500), false, server_options);
 
   // Get parameters for BT timeouts
   int bt_loop_duration;

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -135,10 +135,19 @@ class TimedBehavior : public nav2_core::Behavior
     node->get_parameter("robot_base_frame", robot_base_frame_);
     node->get_parameter("transform_tolerance", transform_tolerance_);
 
+    if (!node->has_parameter("action_server_result_timeout")) {
+      node->declare_parameter("action_server_result_timeout", 10.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), nullptr, std::chrono::milliseconds(
-        500), false);
+        500), false, server_options);
 
     local_collision_checker_ = local_collision_checker;
     global_collision_checker_ = global_collision_checker;

nav2_bringup/params/nav2_params.yaml

@@ -48,6 +48,7 @@ bt_navigator:
     filter_duration: 0.3
     default_server_timeout: 20
     wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
     service_introspection_mode: "disabled"
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
@@ -433,6 +434,7 @@ waypoint_follower:
     loop_rate: 20
     stop_on_failure: false
     service_introspection_mode: "disabled"
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"

nav2_controller/src/controller_server.cpp

@@ -51,6 +51,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 
   declare_parameter("controller_frequency", 20.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_);
   declare_parameter("controller_plugins", default_ids_);
@@ -222,6 +224,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "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);
+
   double costmap_update_timeout_dbl;
   get_parameter("costmap_update_timeout", costmap_update_timeout_dbl);
   costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl);
@@ -235,8 +242,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
       std::bind(&ControllerServer::computeControl, this),
       nullptr,
       std::chrono::milliseconds(500),
-      true /*spin thread*/, rcl_action_server_get_default_options(),
-      use_realtime_priority_ /*soft realtime*/);
+      true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/);
   } catch (const std::runtime_error & e) {
     RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what());
     on_cleanup(state);

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -85,18 +85,25 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("odom_topic", odom_topic);
   odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node, odom_topic);
 
+  double action_server_result_timeout;
+  nav2_util::declare_parameter_if_not_declared(
+    node, "action_server_result_timeout", rclcpp::ParameterValue(10.0));
+  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 dock / undock
   docking_action_server_ = std::make_unique<DockingActionServer>(
     node, "dock_robot",
     std::bind(&DockingServer::dockRobot, this),
     nullptr, std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   undocking_action_server_ = std::make_unique<UndockingActionServer>(
     node, "undock_robot",
     std::bind(&DockingServer::undockRobot, this),
     nullptr, std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   // Create composed utilities
   mutex_ = std::make_shared<std::mutex>();

nav2_planner/src/planner_server.cpp

@@ -52,6 +52,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   // Declare this node's parameters
   declare_parameter("planner_plugins", default_ids_);
   declare_parameter("expected_planner_frequency", 1.0);
+  declare_parameter("action_server_result_timeout", 10.0);
   declare_parameter("costmap_update_timeout", 1.0);
 
   get_parameter("planner_plugins", planner_ids_);
@@ -147,6 +148,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);
+
   double costmap_update_timeout_dbl;
   get_parameter("costmap_update_timeout", costmap_update_timeout_dbl);
   costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl);
@@ -158,15 +164,15 @@ 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(),
     "compute_path_through_poses",
     std::bind(&PlannerServer::computePlanThroughPoses, this),
     nullptr,
     std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }

nav2_smoother/src/nav2_smoother.cpp

@@ -53,6 +53,8 @@ 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_);
+
+  declare_parameter("action_server_result_timeout", 10.0);
 }
 
 SmootherServer::~SmootherServer()
@@ -83,7 +85,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state)
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
   std::string costmap_topic, footprint_topic, robot_base_frame;
-  double transform_tolerance = 0.1;
+  double transform_tolerance;
   this->get_parameter("costmap_topic", costmap_topic);
   this->get_parameter("footprint_topic", footprint_topic);
   this->get_parameter("transform_tolerance", transform_tolerance);
@@ -105,14 +107,19 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & 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(),
     "smooth_path",
     std::bind(&SmootherServer::smoothPlan, this),
     nullptr,
     std::chrono::milliseconds(500),
-    true);
+    true, server_options);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }

nav2_system_tests/src/system/nav2_system_params.yaml

@@ -46,6 +46,7 @@ bt_navigator:
     bt_loop_duration: 10
     filter_duration: 0.3
     default_server_timeout: 20
+    action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
@@ -290,6 +291,7 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"

nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -37,6 +37,8 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options)
   declare_parameter("stop_on_failure", true);
   declare_parameter("loop_rate", 20);
 
+  declare_parameter("action_server_result_timeout", 900.0);
+
   declare_parameter("global_frame_id", "map");
 
   nav2_util::declare_parameter_if_not_declared(
@@ -76,6 +78,10 @@ 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").as_double();
+  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);
+
   xyz_action_server_ = std::make_unique<ActionServer>(
     get_node_base_interface(),
     get_node_clock_interface(),
@@ -84,7 +90,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state)
     "follow_waypoints", std::bind(
       &WaypointFollower::followWaypointsCallback,
       this), nullptr, std::chrono::milliseconds(
-      500), false);
+      500), false, server_options);
 
   from_ll_to_map_client_ = std::make_unique<
     nav2_util::ServiceClient<robot_localization::srv::FromLL,
@@ -102,7 +108,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state)
     std::bind(
       &WaypointFollower::followGPSWaypointsCallback,
       this), nullptr, std::chrono::milliseconds(
-      500), false);
+      500), false, server_options);
 
   try {
     waypoint_task_executor_type_ = nav2_util::get_plugin_type_param(