From f8f42be619744b4e75d13b6c492b908083ef461b Mon Sep 17 00:00:00 2001 From: Pau Carre Cardona Date: Fri, 28 May 2021 15:59:27 +0200 Subject: [PATCH 1/4] action node w.o. exceptions --- .../nav2_behavior_tree/bt_action_node.hpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index eed3626601..853ee696ee 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -138,7 +138,10 @@ class BtActionNode : public BT::ActionNodeBase // user defined callback on_tick(); - on_new_goal_received(); + BT::NodeStatus status = on_new_goal_received(); + if( status == BT::NodeStatus::FAILURE) { + return BT::NodeStatus::FAILURE; + } } // The following code corresponds to the "RUNNING" loop @@ -174,7 +177,9 @@ class BtActionNode : public BT::ActionNodeBase return on_cancelled(); default: - throw std::logic_error("BtActionNode::Tick: invalid status value"); + RCLCPP_ERROR( + node_->get_logger(), "Action %s returned Unknown action status", action_name_.c_str()); + return BT::NodeStatus::FAILURE; } } @@ -213,7 +218,7 @@ class BtActionNode : public BT::ActionNodeBase } - void on_new_goal_received() + BT::NodeStatus on_new_goal_received() { goal_result_available_ = false; auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); @@ -233,13 +238,16 @@ class BtActionNode : public BT::ActionNodeBase if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { - throw std::runtime_error("send_goal failed"); + return BT::NodeStatus::FAILURE; } goal_handle_ = future_goal_handle.get(); if (!goal_handle_) { - throw std::runtime_error("Goal was rejected by the action server"); + RCLCPP_ERROR( + node_->get_logger(), "Goal was rejected by action server %s", action_name_.c_str()); + return BT::NodeStatus::FAILURE; } + return BT::NodeStatus::SUCCESS; } void increment_recovery_count() From bccdd26a1e5afef7ad63e42e4706f13587d73cbc Mon Sep 17 00:00:00 2001 From: Pau Carre Cardona Date: Sat, 29 May 2021 10:13:04 +0200 Subject: [PATCH 2/4] improved logs --- .../include/nav2_behavior_tree/bt_action_node.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 853ee696ee..a44e050b2f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -178,7 +178,7 @@ class BtActionNode : public BT::ActionNodeBase default: RCLCPP_ERROR( - node_->get_logger(), "Action %s returned Unknown action status", action_name_.c_str()); + node_->get_logger(), "Action server \"%s\" returned Unknown action status", action_name_.c_str()); return BT::NodeStatus::FAILURE; } } @@ -194,7 +194,7 @@ class BtActionNode : public BT::ActionNodeBase { RCLCPP_ERROR( node_->get_logger(), - "Failed to cancel action server for %s", action_name_.c_str()); + "Failed to cancel action server \"%s\"", action_name_.c_str()); } } @@ -238,13 +238,16 @@ class BtActionNode : public BT::ActionNodeBase if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR( + node_->get_logger(), "Error sending goal to action server \"%s\" using a timeout of %d milliseconds.", + action_name_.c_str(), server_timeout_); return BT::NodeStatus::FAILURE; } goal_handle_ = future_goal_handle.get(); if (!goal_handle_) { RCLCPP_ERROR( - node_->get_logger(), "Goal was rejected by action server %s", action_name_.c_str()); + node_->get_logger(), "Goal was rejected by action server \"%s\"", action_name_.c_str()); return BT::NodeStatus::FAILURE; } return BT::NodeStatus::SUCCESS; From 7bc07141bf127754cc79c5211813de8d4cca7602 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pau=20Carr=C3=A9=20Cardona?= Date: Fri, 4 Jun 2021 07:19:06 +0200 Subject: [PATCH 3/4] Fix: BTActionNodes fail instead throwing exceptions enabling fallback/recovery mechanisms in BT trees (#1) --- .../nav2_behavior_tree/bt_action_node.hpp | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index eed3626601..a44e050b2f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -138,7 +138,10 @@ class BtActionNode : public BT::ActionNodeBase // user defined callback on_tick(); - on_new_goal_received(); + BT::NodeStatus status = on_new_goal_received(); + if( status == BT::NodeStatus::FAILURE) { + return BT::NodeStatus::FAILURE; + } } // The following code corresponds to the "RUNNING" loop @@ -174,7 +177,9 @@ class BtActionNode : public BT::ActionNodeBase return on_cancelled(); default: - throw std::logic_error("BtActionNode::Tick: invalid status value"); + RCLCPP_ERROR( + node_->get_logger(), "Action server \"%s\" returned Unknown action status", action_name_.c_str()); + return BT::NodeStatus::FAILURE; } } @@ -189,7 +194,7 @@ class BtActionNode : public BT::ActionNodeBase { RCLCPP_ERROR( node_->get_logger(), - "Failed to cancel action server for %s", action_name_.c_str()); + "Failed to cancel action server \"%s\"", action_name_.c_str()); } } @@ -213,7 +218,7 @@ class BtActionNode : public BT::ActionNodeBase } - void on_new_goal_received() + BT::NodeStatus on_new_goal_received() { goal_result_available_ = false; auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); @@ -233,13 +238,19 @@ class BtActionNode : public BT::ActionNodeBase if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { - throw std::runtime_error("send_goal failed"); + RCLCPP_ERROR( + node_->get_logger(), "Error sending goal to action server \"%s\" using a timeout of %d milliseconds.", + action_name_.c_str(), server_timeout_); + return BT::NodeStatus::FAILURE; } goal_handle_ = future_goal_handle.get(); if (!goal_handle_) { - throw std::runtime_error("Goal was rejected by the action server"); + RCLCPP_ERROR( + node_->get_logger(), "Goal was rejected by action server \"%s\"", action_name_.c_str()); + return BT::NodeStatus::FAILURE; } + return BT::NodeStatus::SUCCESS; } void increment_recovery_count() From c62af8ae4fb6b209d2923f352431f5b7acaf1d51 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Tue, 22 Jun 2021 11:15:49 +0200 Subject: [PATCH 4/4] add logs and try catches --- .../nav2_behavior_tree/bt_action_node.hpp | 165 ++++++++++-------- nav2_bt_navigator/src/bt_navigator.cpp | 6 +- nav2_planner/src/planner_server.cpp | 4 +- 3 files changed, 104 insertions(+), 71 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 853ee696ee..4b701cc5f5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -43,6 +43,9 @@ class BtActionNode : public BT::ActionNodeBase config().blackboard->get("server_timeout"); getInput("server_timeout", server_timeout_); + RCLCPP_INFO(node_->get_logger(), "Action Name: %s\nServer Timeout: %d", + action_name_.c_str(), server_timeout_); + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -68,10 +71,20 @@ class BtActionNode : public BT::ActionNodeBase { // Now that we have the ROS node to use, create the action client for this BT action action_client_ = rclcpp_action::create_client(node_, action_name); - - // Make sure the server is actually there before continuing - RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - action_client_->wait_for_action_server(); + if (action_client_ != nullptr) { + // Make sure the server is actually there before continuing + RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); + try { + action_client_->wait_for_action_server(); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node_->get_logger(), "Action server \"%s\" was not available: \"%s\"", action_name.c_str(), ex.what() + ); + } + + } else { + RCLCPP_ERROR(node_->get_logger(), "Action client \"%s\" could not be created, BT node is corrupted", action_name.c_str()); + } } // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method @@ -130,56 +143,61 @@ class BtActionNode : public BT::ActionNodeBase // The main override required by a BT action BT::NodeStatus tick() override { - // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) { - // setting the status to RUNNING to notify the BT Loggers (if any) - setStatus(BT::NodeStatus::RUNNING); - - // user defined callback - on_tick(); - - BT::NodeStatus status = on_new_goal_received(); - if( status == BT::NodeStatus::FAILURE) { - return BT::NodeStatus::FAILURE; + try { + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) { + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + + // user defined callback + on_tick(); + + BT::NodeStatus status = on_new_goal_received(); + if( status == BT::NodeStatus::FAILURE) { + return BT::NodeStatus::FAILURE; + } } - } - - // The following code corresponds to the "RUNNING" loop - if (rclcpp::ok() && !goal_result_available_) { - // user defined callback. May modify the value of "goal_updated_" - on_wait_for_result(); - auto goal_status = goal_handle_->get_status(); - if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) - { - goal_updated_ = false; - on_new_goal_received(); - } + // The following code corresponds to the "RUNNING" loop + if (rclcpp::ok() && !goal_result_available_) { + // user defined callback. May modify the value of "goal_updated_" + on_wait_for_result(); + + auto goal_status = goal_handle_->get_status(); + if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + { + goal_updated_ = false; + on_new_goal_received(); + } - rclcpp::spin_some(node_); + rclcpp::spin_some(node_); - // check if, after invoking spin_some(), we finally received the result - if (!goal_result_available_) { - // Yield this Action, returning RUNNING - return BT::NodeStatus::RUNNING; + // check if, after invoking spin_some(), we finally received the result + if (!goal_result_available_) { + // Yield this Action, returning RUNNING + return BT::NodeStatus::RUNNING; + } } - } - switch (result_.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - return on_success(); + switch (result_.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + return on_success(); - case rclcpp_action::ResultCode::ABORTED: - return on_aborted(); + case rclcpp_action::ResultCode::ABORTED: + return on_aborted(); - case rclcpp_action::ResultCode::CANCELED: - return on_cancelled(); + case rclcpp_action::ResultCode::CANCELED: + return on_cancelled(); - default: - RCLCPP_ERROR( - node_->get_logger(), "Action %s returned Unknown action status", action_name_.c_str()); - return BT::NodeStatus::FAILURE; + default: + RCLCPP_ERROR( + node_->get_logger(), "Action %s returned Unknown action status", action_name_.c_str()); + return BT::NodeStatus::FAILURE; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(node_->get_logger(), "Action %s failed with exception: %s", action_name_.c_str(), ex.what()); + return BT::NodeStatus::FAILURE; } } @@ -187,6 +205,7 @@ class BtActionNode : public BT::ActionNodeBase // make sure to cancel the ROS2 action if it is still running. void halt() override { + RCLCPP_INFO(node_->get_logger(), "BT Action \"%s\" has been halted", action_name_.c_str()); if (should_cancel_goal()) { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) != @@ -204,6 +223,7 @@ class BtActionNode : public BT::ActionNodeBase protected: bool should_cancel_goal() { + RCLCPP_INFO(node_->get_logger(), "BT Action \"%s\" goal should be canceled", action_name_.c_str()); // Shut the node down if it is currently running if (status() != BT::NodeStatus::RUNNING) { return false; @@ -220,34 +240,41 @@ class BtActionNode : public BT::ActionNodeBase BT::NodeStatus on_new_goal_received() { - goal_result_available_ = false; - auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { - // TODO(#1652): a work around until rcl_action interface is updated - // if goal ids are not matched, the older goal call this callback so ignore the result - // if matched, it must be processed (including aborted) - if (this->goal_handle_->get_goal_id() == result.goal_id) { - goal_result_available_ = true; - result_ = result; - } - }; - - auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); - - if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { - return BT::NodeStatus::FAILURE; - } + try { + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + // TODO(#1652): a work around until rcl_action interface is updated + // if goal ids are not matched, the older goal call this callback so ignore the result + // if matched, it must be processed (including aborted) + if (this->goal_handle_->get_goal_id() == result.goal_id) { + goal_result_available_ = true; + result_ = result; + } + }; + + auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + + if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + return BT::NodeStatus::FAILURE; + } - goal_handle_ = future_goal_handle.get(); - if (!goal_handle_) { + goal_handle_ = future_goal_handle.get(); + if (!goal_handle_) { + RCLCPP_ERROR( + node_->get_logger(), "Goal was rejected by action server %s", action_name_.c_str()); + return BT::NodeStatus::FAILURE; + } + return BT::NodeStatus::SUCCESS; + } catch (const std::exception & ex) { RCLCPP_ERROR( - node_->get_logger(), "Goal was rejected by action server %s", action_name_.c_str()); + node_->get_logger(), "New goal for action [%s] was received failed with exception: %s", action_name_.c_str(), ex.what() + ); return BT::NodeStatus::FAILURE; } - return BT::NodeStatus::SUCCESS; } void increment_recovery_count() diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index afd20df0e6..ef731a9ff0 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -316,7 +316,11 @@ BtNavigator::navigateToPose() case nav2_behavior_tree::BtStatus::FAILED: RCLCPP_ERROR(get_logger(), "Navigation failed"); - action_server_->terminate_current(); + try { + action_server_->terminate_current(); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Failed to terminate action after BT sent failed status: %s", ex.what()); + } break; case nav2_behavior_tree::BtStatus::CANCELED: diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 48eede3686..91aeba1d3a 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -222,11 +222,13 @@ PlannerServer::computePlan() geometry_msgs::msg::PoseStamped start; if (!costmap_ros_->getRobotPose(start)) { + RCLCPP_INFO(get_logger(), "Failed to get robot pose in global frame."); action_server_->terminate_current(); return; } if (action_server_->is_preempt_requested()) { + RCLCPP_INFO(get_logger(), "Goal was preempted."); goal = action_server_->accept_pending_goal(); } @@ -241,7 +243,7 @@ PlannerServer::computePlan() return; } - RCLCPP_DEBUG( + RCLCPP_INFO( get_logger(), "Found valid path of size %u to (%.2f, %.2f)", result->path.poses.size(), goal->pose.pose.position.x,