From b71cccd8c47d4cb472db1c3a509af3821336933a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 12 Sep 2024 11:08:18 -0700 Subject: [PATCH] Adding planner server timeout for costmap waiting (#4673) * Adding planner server timeout for costmap waiting Signed-off-by: Steve Macenski * Adding controller server's costmap timeout as well Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .../params/nav2_multirobot_params_1.yaml | 2 ++ .../params/nav2_multirobot_params_2.yaml | 2 ++ .../params/nav2_multirobot_params_all.yaml | 2 ++ nav2_bringup/params/nav2_params.yaml | 2 ++ .../nav2_controller/controller_server.hpp | 1 + nav2_controller/src/controller_server.cpp | 19 ++++++++++++++++++- .../nav2_core/controller_exceptions.hpp | 7 +++++++ nav2_msgs/action/FollowPath.action | 1 + .../include/nav2_planner/planner_server.hpp | 1 + nav2_planner/src/planner_server.cpp | 10 ++++++++++ 10 files changed, 46 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 493c53fc2b..319ebd41ac 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -198,6 +199,7 @@ map_server: planner_server: ros__parameters: planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 605c86c66b..86e2b78e5e 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -197,6 +198,7 @@ map_server: planner_server: ros__parameters: planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 4bde87b47d..b6b27887a3 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -230,6 +231,7 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 + costmap_update_timeout: 1.0 planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2581635e7f..2027f4c0e1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -276,6 +277,7 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 0f250dee14..e2d3fd497a 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -267,6 +267,7 @@ class ControllerServer : public nav2_util::LifecycleNode double failure_tolerance_; bool use_realtime_priority_; + rclcpp::Duration costmap_update_timeout_; // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8ea2d9cfb1..a990c3a400 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -44,7 +44,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, - default_types_{"dwb_core::DWBLocalPlanner"} + default_types_{"dwb_core::DWBLocalPlanner"}, + costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -63,6 +64,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); + declare_parameter("costmap_update_timeout", 0.30); // 300ms // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -222,6 +224,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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); + // Create the action server that we implement with our followPath method // This may throw due to real-time prioritzation if user doesn't have real-time permissions try { @@ -480,7 +486,11 @@ void ControllerServer::computeControl() // Don't compute a trajectory until costmap is valid (after clear costmap) rclcpp::Rate r(100); + auto waiting_start = now(); while (!costmap_ros_->isCurrent()) { + if (now() - waiting_start > costmap_update_timeout_) { + throw nav2_core::ControllerTimedOut("Costmap timed out waiting for update"); + } r.sleep(); } @@ -543,6 +553,13 @@ void ControllerServer::computeControl() result->error_code = Action::Result::INVALID_PATH; action_server_->terminate_current(result); return; + } catch (nav2_core::ControllerTimedOut & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Result::CONTROLLER_TIMED_OUT; + action_server_->terminate_current(result); + return; } catch (nav2_core::ControllerException & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp index 5f87c4c287..2d75836fa5 100644 --- a/nav2_core/include/nav2_core/controller_exceptions.hpp +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -69,6 +69,13 @@ class NoValidControl : public ControllerException : ControllerException(description) {} }; +class ControllerTimedOut : public ControllerException +{ +public: + explicit ControllerTimedOut(const std::string & description) + : ControllerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 49f04e2139..80b0a1d4ef 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -16,6 +16,7 @@ uint16 INVALID_PATH=103 uint16 PATIENCE_EXCEEDED=104 uint16 FAILED_TO_MAKE_PROGRESS=105 uint16 NO_VALID_CONTROL=106 +uint16 CONTROLLER_TIMED_OUT=107 std_msgs/Empty result uint16 error_code diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 12e364c710..2810a89b06 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -242,6 +242,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_ids_; std::vector planner_types_; double max_planner_duration_; + rclcpp::Duration costmap_update_timeout_; std::string planner_ids_concat_; // TF buffer diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index d572360fc9..bb59b0b023 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -44,6 +44,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), default_ids_{"GridBased"}, default_types_{"nav2_navfn_planner::NavfnPlanner"}, + costmap_update_timeout_(1s), costmap_(nullptr) { RCLCPP_INFO(get_logger(), "Creating"); @@ -52,6 +53,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) 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_); if (planner_ids_ == default_ids_) { @@ -150,6 +152,10 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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); + // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( shared_from_this(), @@ -283,7 +289,11 @@ void PlannerServer::waitForCostmap() { // Don't compute a plan until costmap is valid (after clear costmap) rclcpp::Rate r(100); + auto waiting_start = now(); while (!costmap_ros_->isCurrent()) { + if (now() - waiting_start > costmap_update_timeout_) { + throw nav2_core::PlannerTimedOut("Costmap timed out waiting for update"); + } r.sleep(); } }