Populate error_msg for all action result messages (#4341)

.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
+

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

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -50,7 +50,9 @@
   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 @@
     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 @@
   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 @@
 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 @@
   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 @@
         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 @@
 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 @@
 
     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 @@
   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

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"),
       });
   }
 

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"),
       });
   }
 };

nav2_behavior_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"),
       });
   }
 };

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"),
       });
   }
 };

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"),
       });
   }
 

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"),
       });
   }
 };

nav2_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"),
       });
   }
 };

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"),
       });
   }
 };

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"),
       });
   }
 };