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 d2b120f44cb..0a6aa511b49 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 @@ -268,7 +268,10 @@ template void BtActionServer::executeCallback() { if (!on_goal_received_callback_(action_server_->get_current_goal())) { - action_server_->terminate_current(); + // Give server an opportunity to populate the result message + auto result = std::make_shared(); + populateErrorCode(result); + action_server_->terminate_current(result); cleanErrorCodes(); return; } @@ -333,6 +336,7 @@ void BtActionServer::populateErrorCode( typename std::shared_ptr result) { int highest_priority_error_code = std::numeric_limits::max(); + std::string highest_priority_error_msg = ""; for (const auto & error_code : error_code_names_) { try { int current_error_code = blackboard_->get(error_code); @@ -349,6 +353,7 @@ void BtActionServer::populateErrorCode( if (highest_priority_error_code != std::numeric_limits::max()) { result->error_code = highest_priority_error_code; + result->error_msg = highest_priority_error_msg; } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index 97511608d50..6352ad24b80 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -80,7 +80,9 @@ class AssistedTeleopAction : public BtActionNode("time_allowance", 10.0, "Allowed time for running assisted teleop"), BT::InputPort("is_recovery", false, "If true the recovery count will be incremented"), BT::OutputPort( - "error_code_id", "The assisted teleop behavior server error code") + "error_code_id", "The assisted teleop behavior server error code"), + BT::OutputPort( + "error_msg", "The assisted teleop behavior server error msg"), }); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp index 1419c40661d..f06aa711a44 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp @@ -81,7 +81,9 @@ class BackUpAction : public BtActionNode BT::InputPort("backup_speed", 0.025, "Speed at which to backup"), BT::InputPort("time_allowance", 10.0, "Allowed time for reversing"), BT::OutputPort( - "error_code_id", "The back up behavior server error code") + "error_code_id", "The back up behavior server error code"), + BT::OutputPort( + "error_msg", "The back up behavior server error msg"), }); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index fe5d2e9dd25..864c5049e24 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -86,6 +86,8 @@ class ComputePathThroughPosesAction BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::OutputPort( "error_code_id", "The compute path through poses error code"), + BT::OutputPort( + "error_msg", "The compute path through poses error msg"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 07e9a4a2f2f..a196ac42174 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -86,6 +86,8 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::OutputPort( "error_code_id", "The compute path to pose error code"), + BT::OutputPort( + "error_msg", "The compute path to pose error msg"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index dd1a6f29086..366584f7862 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -60,7 +60,9 @@ class DriveOnHeadingAction : public BtActionNode("speed", 0.025, "Speed at which to travel"), BT::InputPort("time_allowance", 10.0, "Allowed time for driving on heading"), BT::OutputPort( - "error_code_id", "The drive on heading behavior server error code") + "error_code_id", "The drive on heading behavior server error code"), + BT::OutputPort( + "error_msg", "The drive on heading behavior server error msg"), }); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index c05c2c0d59f..6e64de8f7d4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -86,6 +86,8 @@ class FollowPathAction : public BtActionNode BT::InputPort("progress_checker_id", ""), BT::OutputPort( "error_code_id", "The follow path error code"), + BT::OutputPort( + "error_msg", "The follow path error msg"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 44ad055ea24..d713b62bfd6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -79,6 +79,8 @@ class NavigateThroughPosesAction : public BtActionNode("behavior_tree", "Behavior tree to run"), BT::OutputPort( "error_code_id", "The navigate through poses error code"), + BT::OutputPort( + "error_msg", "The navigate through poses error msg"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index 849c8259a48..216fbd43c5c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -77,6 +77,8 @@ class NavigateToPoseAction : public BtActionNode("behavior_tree", "Behavior tree to run"), BT::OutputPort( "error_code_id", "Navigate to pose error code"), + BT::OutputPort( + "error_msg", "Navigate to pose error msg"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp index d5ec4ced047..8e34d335313 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -87,6 +87,8 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode( "error_code_id", "The smooth path error code"), + BT::OutputPort( + "error_msg", "The smooth path error msg"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 4f9b300e445..44f8c172015 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -65,7 +65,9 @@ class SpinAction : public BtActionNode BT::InputPort("time_allowance", 10.0, "Allowed time for spinning"), BT::InputPort("is_recovery", true, "True if recovery"), BT::OutputPort( - "error_code_id", "The spin behavior error code") + "error_code_id", "The spin behavior error code"), + BT::OutputPort( + "error_msg", "The spin behavior error msg"), }); } diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index e876d2ce40b..f7e643b82fa 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -53,18 +53,21 @@ void AssistedTeleopAction::on_tick() BT::NodeStatus AssistedTeleopAction::on_success() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus AssistedTeleopAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS; } BT::NodeStatus AssistedTeleopAction::on_cancelled() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index e17580fe71c..2875d9b7c49 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -59,18 +59,21 @@ void BackUpAction::on_tick() BT::NodeStatus BackUpAction::on_success() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus BackUpAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } BT::NodeStatus BackUpAction::on_cancelled() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index a0572ecad61..55540efb72d 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -43,6 +43,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success() setOutput("path", result_.result->path); // Set empty error code, action was successful setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } @@ -51,6 +52,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } @@ -60,6 +62,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() setOutput("path", empty_path); // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index c7f1a0164e8..fb5fdd8202e 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -42,6 +42,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() setOutput("path", result_.result->path); // Set empty error code, action was successful setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } @@ -50,6 +51,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } @@ -59,6 +61,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() setOutput("path", empty_path); // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index 03c00344141..5b7a86b4c82 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -57,18 +57,21 @@ void DriveOnHeadingAction::on_tick() BT::NodeStatus DriveOnHeadingAction::on_success() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus DriveOnHeadingAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } BT::NodeStatus DriveOnHeadingAction::on_cancelled() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index b662221de0f..081f36caee0 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -39,12 +39,14 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus FollowPathAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } @@ -52,6 +54,7 @@ BT::NodeStatus FollowPathAction::on_cancelled() { // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 9213cd564f4..be6272317ab 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -42,12 +42,14 @@ void NavigateThroughPosesAction::on_tick() BT::NodeStatus NavigateThroughPosesAction::on_success() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus NavigateThroughPosesAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } @@ -55,6 +57,7 @@ BT::NodeStatus NavigateThroughPosesAction::on_cancelled() { // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 07bea22436f..4bfac9e8829 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -41,12 +41,14 @@ void NavigateToPoseAction::on_tick() BT::NodeStatus NavigateToPoseAction::on_success() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus NavigateToPoseAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } @@ -54,6 +56,7 @@ BT::NodeStatus NavigateToPoseAction::on_cancelled() { // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index 3a67904558b..ebd72cbf137 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -46,12 +46,14 @@ BT::NodeStatus SmoothPathAction::on_success() setOutput("was_completed", result_.result->was_completed); // Set empty error code, action was successful setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus SmoothPathAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } @@ -59,6 +61,7 @@ BT::NodeStatus SmoothPathAction::on_cancelled() { // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index d10bb83f32b..920384c065b 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -55,18 +55,21 @@ void SpinAction::on_tick() BT::NodeStatus SpinAction::on_success() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; } BT::NodeStatus SpinAction::on_aborted() { setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; } BT::NodeStatus SpinAction::on_cancelled() { setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; }