Populate error_msg for all action result messages (#4341) (#4460)

* Populate error_msg in action result messages (#4341) Signed-off-by: Mike Wake <[email protected]> * FollowWaypoints - Add usage of error_code and error_msg (#4341) Introduces error codes:- - NO_WAYPOINTS_GIVEN - STOP_ON_MISSED_WAYPOINT Signed-off-by: Mike Wake <[email protected]> * nav2_simple_commander: use error_code and error_msg (#4341) Signed-off-by: Mike Wake <[email protected]> * Create and populate BT::OutputPort(s) for error_msg (#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_msg handling to nav2_bt_navigators (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_simple_command track LastActionError (#4341) Only actions provide error_code and error_msg in their result msg. Signed-off-by: Mike Wake <[email protected]> * Restore output format of planner server message (#4341) Signed-off-by: Mike Wake <[email protected]> * Fix opennav_docking error_msg/code usage (#4341) Signed-off-by: Mike Wake <[email protected]> * Fix error_msg type -> BT::OutputPort<std::string> (#4341) Signed-off-by: Mike Wake <[email protected]> * report index of failed initializeGoalPoses (#4341) Add the index of the pose that cannot be transformed in NavigateThroughPosesNavigator::initializeGoalPoses to the error_msg Signed-off-by: Mike Wake <[email protected]> * Add error_msg to (un)dock_robot bt (#4341) Signed-off-by: Mike Wake <[email protected]> * fix dock/undock use of error_msg (#4341) Signed-off-by: Mike Wake <[email protected]> * Improve comment to populate error_msg if goal not accepted (#4341) Signed-off-by: Mike Wake <[email protected]> * change robot_navigator api to (get|set|clear)TaskError() (#4341) Signed-off-by: Mike Wake <[email protected]> * remove exceptions from navigator onLoop (#4341) Signed-off-by: Mike Wake <[email protected]> * change error code to GOAL_TRANSFORMATION_ERROR (#4341) Signed-off-by: Mike Wake <[email protected]> * check getCurrentPose in initialiseGoalPoses (#4341) This mirrors the check done in NavigateToPose::initializeGoalPose checking that we can get a current pose in order to not accept the goal request. Signed-off-by: Mike Wake <[email protected]> * change error_code_names_ to error_code_name_prefixes_ (#4341) Signed-off-by: Mike Wake <[email protected]> * add error_msg port to default behavior xml files (#4341) Signed-off-by: Mike Wake <[email protected]> * Fix 'basic_string::_M_construct null not valid' exception(#4341) This was tough to spot. The nav2_system_tests waypoint follower provided a reproducable test Found out how to run just the waypoint_follower test colcon test --packages-select nav2_system_tests --ctest-args -N --event-handler console_direct+ | grep waypoint_follower changed src/navigation2/nav2_bringup/launch/navigation_launch.py to start nav2_bt_navigator Node with + prefix=['gnome-terminal -- gdb -ex run --args'], + respawn=False, Trying to use gdb to catch where the exception was thrown is too slow and the test gives up on itself. I found the relevant throw std::logic_error on my system by writing tiny program and breaking on std::logic_error int main() { const char *a = nullptr; try { std::string b(a); } catch (const std::logic_error &ex) { std::cout << "logic_error:" << ex.what() << std::endl; } catch (const std::exception &ex) { std::cout << "some other type:" << ex.what() << std::endl; } } Turns out it get thrown here on my system /usr/include/c++/11/bits/basic_string.tcc:212 Created ~/.gdbinit set breakpoint pending on break /usr/include/c++/11/bits/basic_string.tcc:212 This specific breakpoint on a line works fast enough for the integration test to throw the 'basic_string::_M_construct null not valid' and the silly initialisation mistake to be found. Signed-off-by: Mike Wake <[email protected]> * fix error_code_name_prefixes: in nav2_system_param.yml (#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_msg handling to builtin nav2_behaviors plugins (#4341) Signed-off-by: Mike Wake <[email protected]> * simple_action_server improve "Aborting handle" log (#4341) Use SFINAE to log get_error_details_if_availble() in ActionT::Result Ideally all Action messages would have error_code and error_msg fields in their Result. BUT the tests use standard messages that do not and will not. i.e. test_msgs/action/Fibonacci.action Some Action messages only have error_code and not error_msg Signed-off-by: Mike Wake <[email protected]> * Add vim temporary files to .gitignore Signed-off-by: Mike Wake <[email protected]> * Fix include what you use (#4341) Signed-off-by: Mike Wake <[email protected]> * Fix cpplint and uncrustify (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_waypoint_follower do is_premppt_requested() before is_cancel_requested() (4341)(4602) Signed-off-by: Mike Wake <[email protected]> * fix behavior tree xml Backup backup_code_error_msg (#4341) Signed-off-by: Mike Wake <[email protected]> * Fix internal error tracking of navigators (#4341) Key point is only use the internal_error_(code|msg)_ if the action result error_code is 0. Signed-off-by: Mike Wake <[email protected]> * fix halt() in ComputePathToPose ComputePathThroughPoses (#4341) Clear output path for ComputePathThroughPoses similar to ComputePathToPose Explicitly note NOT to clear error_code_id and error_msg behavior tree output ports. Otherwise we cannot read them when the navigator reports errors. Signed-off-by: Mike Wake <[email protected]> * fix NavigateToPose onLoop current_path blackboard existance logic (#4341) Make it consistent with NavigateThroughPoses. This was introduced when changing logic to remove internal exception. Signed-off-by: Mike Wake <[email protected]> * nav2_msgs MissedWaypoint interface change, add error_msg (#4341) Allow of reason for missed waypoint as error_msg. Signed-off-by: Mike Wake <[email protected]> * Fix waypoint_follower error_msg handling (#4341) Include error_msg in reason for missed waypoint Signed-off-by: Mike Wake <[email protected]> * Fix cpplint uncrustify (#4341) Signed-off-by: Mike Wake <[email protected]> * Add tool to help check for error_code and error_msg in behavior trees (#4341) Signed-off-by: Mike Wake <[email protected]> * controller_server: Remove redundant logging (#4341) Signed-off-by: Mike Wake <[email protected]> * timed_behaviour: pass through on_run_result.error_msg (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_waypoint_follower - revert logic swap experiment (#4341) It was a red herring in search for error_msg clearing bug on abort Signed-off-by: Mike Wake <[email protected]> * nav2_waypoint_follower: clear error_msg when clearing error_code (#4341) Signed-off-by: Mike Wake <[email protected]> * Remove tool to check behaviour tree error_msg compliance (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_simple_commander: improve goal request rejection error reporting (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: ensure error_msg not empty (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: behaviour_tree error_code, error_msg checks (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: behaviour_tree error_code, error_msg checks (#4341) Signed-off-by: Mike Wake <[email protected]> * doc: fix hint on how to run bt_navigator tests with ctest. Use -R to invoke the regular expresion. Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: log error_code and error_msg. (#4341) Signed-off-by: Mike Wake <[email protected]> * lint: remove tab (#4341) Signed-off-by: Mike Wake <[email protected]> * add result->error_msg for new CONTROLLER_TIMED_OUT (#4341) Signed-off-by: Mike Wake <[email protected]> * Add error codes NONE and UNKNOWN to Wait.action (#4341) This allows setting a default failure error_code and message in nav2_system_tests behavior_tree dummy_action_server Signed-off-by: Mike Wake <[email protected]> * move internal error tracking into bt_action_server.hpp and test (#4341) Signed-off-by: Mike Wake <[email protected]> * uncrustify and flake8 (#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests behavior_tree result handling improvement (#4341) NOTE: This stuff does not really test the error_msg/error_code handling as its all dummy servers. Signed-off-by: Mike Wake <[email protected]> * Add ProgressCheckerSelector and GoalCheckerSelector to navigate_through_poses_w_replanning_and_recovery.xml (#4341) To facilitate error_msg system tests allow progress and goal checkers to be changed via the behaviour tree mechanism. Signed-off-by: Mike Wake <[email protected]> * Add nav2_system_tests for controller_server/behaviour tree error_msg (#4341) Signed-off-by: Mike Wake <[email protected]> * Improve README hint on how to run error_msg tests (#4341) Signed-off-by: Mike Wake <[email protected]> * fix PostStampedArray usage in nav_through_poses_tester_error_msg (#4341) Signed-off-by: Mike Wake <[email protected]> * nav_through_poses_tester_error_msg - towards clean pyright (#4341) Signed-off-by: Mike Wake <[email protected]> * SmoothPath fix error_(code_id|msg) output ports nav2_tree_nodes.xml (#4341) Signed-off-by: Mike Wake <[email protected]> * Wait action add error_(code_id|msg) output ports (#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_code_id and error_msg ports to example behavior_tree xml (#4341) Signed-off-by: Mike Wake <[email protected]> * Add more default error_code_name_prefixes (#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_(code_id|msg) to opennav_docking_bt example (#4341) Signed-off-by: Mike Wake <[email protected]> * error_msg review fixes (#4341) Signed-off-by: Mike Wake <[email protected]> * Adjust Navigator error code ranges (#4341) Signed-off-by: Mike Wake <[email protected]> * Fix Navigate(ToPose|ThroughPoses)Navigator::goalCompleted warning log (#4341) Signed-off-by: Mike Wake <[email protected]> --------- Signed-off-by: Mike Wake <[email protected]> Signed-off-by: Mike Wake <[email protected]> Co-authored-by: Mike Wake <[email protected]>
ros-navigation · Mar 4, 2025 · 7e3ae16 · 7e3ae16
1 parent 3ddf03c
commit 7e3ae16
Show file tree

Hide file tree

Showing 90 changed files with 1,370 additions and 370 deletions.
diff --git a/.gitignore b/.gitignore
@@ -54,3 +54,19 @@ cmake-build-debug/
 
 # doxygen docs
 doc/html/
+# Vim Swap
+[._]*.s[a-v][a-z]
+[._]*.sw[a-p]
+[._]s[a-rt-v][a-z]
+[._]ss[a-gi-z]
+[._]sw[a-p]
+
+# Vim Persistent undo
+[._]*.un~
+
+# Vim Session
+Session.vim
+
+# Vim Temporary
+.netrwhist
+
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -188,6 +188,25 @@ class BtActionServer
     tree_.haltTree();
   }
 
+  /**
+   * @brief Set internal error code and message
+   * @param error_code the internal error code
+   * @param error_msg the internal error message
+   */
+  void setInternalError(uint16_t error_code, const std::string & error_msg);
+
+  /**
+   * @brief reset internal error code and message
+   */
+  void resetInternalError(void);
+
+  /**
+   * @brief populate result with internal error code and error_msg if not NONE
+   * @param result the action server result to be updated
+   * @return bool action server result was changed
+   */
+  bool populateInternalError(typename std::shared_ptr<typename ActionT::Result> result);
+
 protected:
   /**
    * @brief Action server callback
@@ -228,8 +247,8 @@ class BtActionServer
   // Libraries to pull plugins (BT Nodes) from
   std::vector<std::string> plugin_lib_names_;
 
-  // Error code id names
-  std::vector<std::string> error_code_names_;
+  // Error code name prefixes
+  std::vector<std::string> error_code_name_prefixes_;
 
   // A regular, non-spinning ROS node that we can use for calls to the action client
   rclcpp::Node::SharedPtr client_node_;
@@ -263,6 +282,10 @@ class BtActionServer
   OnLoopCallback on_loop_callback_;
   OnPreemptCallback on_preempt_callback_;
   OnCompletionCallback on_completion_callback_;
+
+  // internal error tracking (IOW not behaviorTree blackboard errors)
+  uint16_t internal_error_code_;
+  std::string internal_error_msg_;
 };
 
 }  // namespace nav2_behavior_tree

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
@@ -50,7 +50,9 @@ BtActionServer<ActionT>::BtActionServer(
   on_goal_received_callback_(on_goal_received_callback),
   on_loop_callback_(on_loop_callback),
   on_preempt_callback_(on_preempt_callback),
-  on_completion_callback_(on_completion_callback)
+  on_completion_callback_(on_completion_callback),
+  internal_error_code_(0),
+  internal_error_msg_()
 {
   auto node = node_.lock();
   logger_ = node->get_logger();
@@ -73,34 +75,51 @@ BtActionServer<ActionT>::BtActionServer(
     node->declare_parameter("wait_for_service_timeout", 1000);
   }
 
-  std::vector<std::string> error_code_names = {
-    "follow_path_error_code",
-    "compute_path_error_code"
+  std::vector<std::string> error_code_name_prefixes = {
+    "assisted_teleop",
+    "backup",
+    "compute_path",
+    "dock_robot",
+    "drive_on_heading",
+    "follow_path",
+    "nav_thru_poses",
+    "nav_to_pose",
+    "spin",
+    "undock_robot",
+    "wait",
   };
 
-  if (!node->has_parameter("error_code_names")) {
+  if (node->has_parameter("error_code_names")) {
+    throw std::runtime_error("parameter 'error_code_names' has been replaced by "
+      " 'error_code_name_prefixes' and MUST be removed.\n"
+      " Please review migration guide and update your configuration.");
+  }
+
+  if (!node->has_parameter("error_code_name_prefixes")) {
     const rclcpp::ParameterValue value = node->declare_parameter(
-      "error_code_names",
+      "error_code_name_prefixes",
       rclcpp::PARAMETER_STRING_ARRAY);
     if (value.get_type() == rclcpp::PARAMETER_NOT_SET) {
-      std::string error_codes_str;
-      for (const auto & error_code : error_code_names) {
-        error_codes_str += " " + error_code;
+      std::string error_code_name_prefixes_str;
+      for (const auto & error_code_name_prefix : error_code_name_prefixes) {
+        error_code_name_prefixes_str += " " + error_code_name_prefix;
       }
       RCLCPP_WARN_STREAM(
-        logger_, "Error_code parameters were not set. Using default values of:"
-          << error_codes_str + "\n"
+        logger_, "error_code_name_prefixes parameters were not set. Using default values of:"
+          << error_code_name_prefixes_str + "\n"
           << "Make sure these match your BT and there are not other sources of error codes you"
           "reported to your application");
-      rclcpp::Parameter error_code_names_param("error_code_names", error_code_names);
-      node->set_parameter(error_code_names_param);
+      rclcpp::Parameter error_code_name_prefixes_param("error_code_name_prefixes",
+        error_code_name_prefixes);
+      node->set_parameter(error_code_name_prefixes_param);
     } else {
-      error_code_names = value.get<std::vector<std::string>>();
-      std::string error_codes_str;
-      for (const auto & error_code : error_code_names) {
-        error_codes_str += " " + error_code;
+      error_code_name_prefixes = value.get<std::vector<std::string>>();
+      std::string error_code_name_prefixes_str;
+      for (const auto & error_code_name_prefix : error_code_name_prefixes) {
+        error_code_name_prefixes_str += " " + error_code_name_prefix;
       }
-      RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str);
+      RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:"
+        << error_code_name_prefixes_str);
     }
   }
 }
@@ -172,7 +191,7 @@ bool BtActionServer<ActionT>::on_configure()
   node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_);
 
   // Get error code id names to grab off of the blackboard
-  error_code_names_ = node->get_parameter("error_code_names").as_string_array();
+  error_code_name_prefixes_ = node->get_parameter("error_code_name_prefixes").as_string_array();
 
   // Create the class that registers our custom nodes and executes the BT
   bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_, client_node_);
@@ -194,6 +213,7 @@ bool BtActionServer<ActionT>::on_configure()
 template<class ActionT>
 bool BtActionServer<ActionT>::on_activate()
 {
+  resetInternalError();
   if (!loadBehaviorTree(default_bt_xml_filename_)) {
     RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str());
     return false;
@@ -239,7 +259,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   std::ifstream xml_file(filename);
 
   if (!xml_file.good()) {
-    RCLCPP_ERROR(logger_, "Couldn't open input XML file: %s", filename.c_str());
+    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+      "Couldn't open input XML file: " + filename);
     return false;
   }
 
@@ -256,7 +277,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
         wait_for_service_timeout_);
     }
   } catch (const std::exception & e) {
-    RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
+    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+      std::string("Exception when loading BT: ") + e.what());
     return false;
   }
 
@@ -270,7 +292,11 @@ template<class ActionT>
 void BtActionServer<ActionT>::executeCallback()
 {
   if (!on_goal_received_callback_(action_server_->get_current_goal())) {
-    action_server_->terminate_current();
+    // Give server an opportunity to populate the result message
+    // if the goal is not accepted
+    auto result = std::make_shared<typename ActionT::Result>();
+    populateErrorCode(result);
+    action_server_->terminate_current(result);
     cleanErrorCodes();
     return;
   }
@@ -318,7 +344,8 @@ void BtActionServer<ActionT>::executeCallback()
 
     case nav2_behavior_tree::BtStatus::FAILED:
       action_server_->terminate_current(result);
-      RCLCPP_ERROR(logger_, "Goal failed");
+      RCLCPP_ERROR(logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
+        result->error_msg.c_str());
       break;
 
     case nav2_behavior_tree::BtStatus::CANCELED:
@@ -330,36 +357,81 @@ void BtActionServer<ActionT>::executeCallback()
   cleanErrorCodes();
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::setInternalError(uint16_t error_code, const std::string & error_msg)
+{
+  internal_error_code_ = error_code;
+  internal_error_msg_ = error_msg;
+  RCLCPP_ERROR(logger_, "Setting internal error error_code:%d, error_msg:%s",
+    internal_error_code_, internal_error_msg_.c_str());
+}
+
+template<class ActionT>
+void BtActionServer<ActionT>::resetInternalError(void)
+{
+  internal_error_code_ = ActionT::Result::NONE;
+  internal_error_msg_ = "";
+}
+
+template<class ActionT>
+bool BtActionServer<ActionT>::populateInternalError(
+  typename std::shared_ptr<typename ActionT::Result> result)
+{
+  if (internal_error_code_ != ActionT::Result::NONE) {
+    result->error_code = internal_error_code_;
+    result->error_msg = internal_error_msg_;
+    return true;
+  }
+  return false;
+}
+
 template<class ActionT>
 void BtActionServer<ActionT>::populateErrorCode(
   typename std::shared_ptr<typename ActionT::Result> result)
 {
   int highest_priority_error_code = std::numeric_limits<int>::max();
-  for (const auto & error_code : error_code_names_) {
+  std::string highest_priority_error_msg = "";
+  std::string name;
+
+  if (internal_error_code_ != 0) {
+    highest_priority_error_code = internal_error_code_;
+    highest_priority_error_msg = internal_error_msg_;
+  }
+
+  for (const auto & error_code_name_prefix : error_code_name_prefixes_) {
     try {
-      int current_error_code = blackboard_->get<int>(error_code);
+      name = error_code_name_prefix + "_error_code";
+      int current_error_code = blackboard_->get<int>(name);
       if (current_error_code != 0 && current_error_code < highest_priority_error_code) {
         highest_priority_error_code = current_error_code;
+        name = error_code_name_prefix + "_error_msg";
+        highest_priority_error_msg = blackboard_->get<std::string>(name);
       }
     } catch (...) {
       RCLCPP_DEBUG(
         logger_,
-        "Failed to get error code: %s from blackboard",
-        error_code.c_str());
+        "Failed to get error code name: %s from blackboard",
+        name.c_str());
     }
   }
 
   if (highest_priority_error_code != std::numeric_limits<int>::max()) {
     result->error_code = highest_priority_error_code;
+    result->error_msg = highest_priority_error_msg;
   }
 }
 
 template<class ActionT>
 void BtActionServer<ActionT>::cleanErrorCodes()
 {
-  for (const auto & error_code : error_code_names_) {
-    blackboard_->set<unsigned short>(error_code, 0);  //NOLINT
+  std::string name;
+  for (const auto & error_code_name_prefix : error_code_name_prefixes_) {
+    name = error_code_name_prefix + "_error_code";
+    blackboard_->set<unsigned short>(name, 0);  //NOLINT
+    name = error_code_name_prefix + "_error_msg";
+    blackboard_->set<std::string>(name, "");
   }
+  resetInternalError();
 }
 
 }  // namespace nav2_behavior_tree

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
@@ -80,7 +80,9 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for running assisted teleop"),
         BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented"),
         BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The assisted teleop behavior server error code")
+          "error_code_id", "The assisted teleop behavior server error code"),
+        BT::OutputPort<std::string>(
+          "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
@@ -82,7 +82,9 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
         BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The back up behavior server error code")
+          "error_code_id", "The back up behavior server error code"),
+        BT::OutputPort<std::string>(
+          "error_msg", "The back up behavior server error msg"),
       });
   }
 };

diff --git a/...vior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/...vior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
@@ -67,6 +67,11 @@ class ComputePathThroughPosesAction
    */
   BT::NodeStatus on_cancelled() override;
 
+  /**
+   * \brief Override required by the a BT action. Cancel the action and set the path output
+   */
+  void halt() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -86,6 +91,8 @@ class ComputePathThroughPosesAction
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The compute path through poses error code"),
+        BT::OutputPort<std::string>(
+          "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
@@ -86,6 +86,8 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The compute path to pose error code"),
+        BT::OutputPort<std::string>(
+          "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
@@ -61,7 +61,9 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
         BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<Action::Result::_error_code_type>(
-          "error_code_id", "The drive on heading behavior server error code")
+          "error_code_id", "The drive on heading behavior server error code"),
+        BT::OutputPort<std::string>(
+          "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
@@ -86,6 +86,8 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         BT::InputPort<std::string>("progress_checker_id", ""),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The follow path error code"),
+        BT::OutputPort<std::string>(
+          "error_msg", "The follow path error msg"),
       });
   }
 };

diff --git a/...behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/...behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
@@ -78,6 +78,8 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
         BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The navigate through poses error code"),
+        BT::OutputPort<std::string>(
+          "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
@@ -77,6 +77,8 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
         BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "Navigate to pose error code"),
+        BT::OutputPort<std::string>(
+          "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
@@ -87,6 +87,8 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
           "was_completed", "True if smoothing was not interrupted by time limit"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The smooth path error code"),
+        BT::OutputPort<std::string>(
+          "error_msg", "The smooth path error msg"),
       });
   }
 };