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

@@ -228,8 +228,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_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -73,34 +73,41 @@ 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 = {
+    "follow_path",
+    "compute_path",
   };
 
-  if (!node->has_parameter("error_code_names")) {
+  if (node->has_parameter("error_code_names")) {
+    RCLCPP_ERROR(logger_, "parameter 'error_code_names' has been replaced by "
+      " 'error_code_name_prefixes'. Please update");
+  }
+
+  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 +179,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_);
@@ -270,7 +277,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 +329,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:
@@ -335,30 +347,40 @@ 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;
+  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, "");
   }
 }
 

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

@@ -81,7 +81,9 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
         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

@@ -60,7 +60,9 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
         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

@@ -79,6 +79,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"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

@@ -65,7 +65,9 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
         BT::InputPort<bool>("is_recovery", true, "True if recovery"),
         BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The spin behavior error code")
+          "error_code_id", "The spin behavior error code"),
+        BT::OutputPort<std::string>(
+          "error_msg", "The spin behavior error msg"),
       });
   }
 

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -14,6 +14,7 @@
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">"Back up error code"</output_port>
+      <output_port name="error_msg">"Back up error message"</output_port>
     </Action>
 
     <Action ID="DriveOnHeading">
@@ -23,6 +24,7 @@
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">"Drive on heading error code"</output_port>
+      <output_port name="error_msg">"Drive on heading error message"</output_port>
     </Action>
 
     <Action ID="CancelControl">
@@ -80,6 +82,7 @@
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
       <output_port name="error_code_id">"Compute path to pose error code"</output_port>
+      <output_port name="error_msg">"Compute path to pose error message"</output_port>
     </Action>
 
     <Action ID="ComputePathThroughPoses">
@@ -90,6 +93,7 @@
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
       <output_port name="error_code_id">"Compute path through poses error code"</output_port>
+      <output_port name="error_msg">"Compute path through poses error message"</output_port>
     </Action>
 
     <Action ID="RemovePassedGoals">
@@ -126,6 +130,7 @@
       <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">Follow Path error code</output_port>
+      <output_port name="error_msg">Follow Path error message</output_port>
     </Action>
 
     <Action ID="NavigateToPose">
@@ -134,6 +139,7 @@
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
         <output_port name="error_code_id">Navigate to pose error code</output_port>
+        <output_port name="error_msg">Navigate to pose error message</output_port>
     </Action>
 
     <Action ID="NavigateThroughPoses">
@@ -142,6 +148,7 @@
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
         <output_port name="error_code_id">Navigate through poses error code</output_port>
+        <output_port name="error_msg">Navigate through poses error message</output_port>
     </Action>
 
     <Action ID="ReinitializeGlobalLocalization">
@@ -202,6 +209,7 @@
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">Spin error code</output_port>
+      <output_port name="error_msg">Spin error message</output_port>
     </Action>
 
     <Action ID="Wait">
@@ -216,6 +224,7 @@
       <input_port name="server_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">Assisted teleop error code</output_port>
+      <output_port name="error_msg">Assisted teleop error message</output_port>
     </Action>
 
     <!-- ############################### CONDITION NODES ############################## -->

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;
 }
 

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;
 }