error_msg review fixes (ros-navigation#4341)

Signed-off-by: Mike Wake <[email protected]>

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -201,11 +201,11 @@ class BtActionServer
   void resetInternalError(void);
 
   /**
-   * @brief set result to internal error code and message if not NONE
+   * @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 setResultToInternalError(typename std::shared_ptr<typename ActionT::Result> result);
+  bool populateInternalError(typename std::shared_ptr<typename ActionT::Result> result);
 
 protected:
   /**

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -90,8 +90,9 @@ BtActionServer<ActionT>::BtActionServer(
   };
 
   if (node->has_parameter("error_code_names")) {
-    RCLCPP_ERROR(logger_, "parameter 'error_code_names' has been replaced by "
-      " 'error_code_name_prefixes'. Please update");
+    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")) {
@@ -206,14 +207,13 @@ bool BtActionServer<ActionT>::on_configure()
     "wait_for_service_timeout",
     wait_for_service_timeout_);
 
-  resetInternalError();
-
   return true;
 }
 
 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;
@@ -374,7 +374,7 @@ void BtActionServer<ActionT>::resetInternalError(void)
 }
 
 template<class ActionT>
-bool BtActionServer<ActionT>::setResultToInternalError(
+bool BtActionServer<ActionT>::populateInternalError(
   typename std::shared_ptr<typename ActionT::Result> result)
 {
   if (internal_error_code_ != ActionT::Result::NONE) {

nav2_bringup/params/nav2_params.yaml

@@ -70,6 +70,7 @@ bt_navigator:
       - follow_path
       - nav_thru_poses
       - nav_to_pose
+      - route
       - spin
       - undock_robot
       - wait

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -45,7 +45,6 @@ NavigateThroughPosesNavigator::configure(
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;
 
-  bt_action_server_->resetInternalError();
   return true;
 }
 
@@ -89,13 +88,15 @@ NavigateThroughPosesNavigator::goalCompleted(
   typename ActionT::Result::SharedPtr result,
   const nav2_behavior_tree::BtStatus /*final_bt_status*/)
 {
-  if (result->error_code == 0 && bt_action_server_->setResultToInternalError(result)) {
-    RCLCPP_WARN(logger_,
-      "NavigateThroughPosesNavigator::goalCompleted, set result to internal error %d:'%s'.",
-      result->error_code,
-      result->error_msg.c_str());
+  if (result->error_code == 0) {
+    if (bt_action_server_->populateInternalError(result)) {
+      RCLCPP_WARN(logger_,
+        "NavigateThroughPosesNavigator::goalCompleted, internal error %d:'%s'.",
+        result->error_code,
+        result->error_msg.c_str());
+    }
   } else {
-    RCLCPP_INFO(logger_, "NavigateThroughPosesNavigator::goalCompleted result %d:'%s'.",
+    RCLCPP_INFO(logger_, "NavigateThroughPosesNavigator::goalCompleted error %d:'%s'.",
       result->error_code,
       result->error_msg.c_str());
   }
@@ -126,7 +127,7 @@ NavigateThroughPosesNavigator::onLoop()
       feedback_utils_.global_frame, feedback_utils_.robot_frame,
       feedback_utils_.transform_tolerance))
   {
-    // Robot pose is not yet available, no feedback
+    RCLCPP_ERROR(logger_, "Robot pose is not available.");
     return;
   }
 
@@ -222,7 +223,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
       feedback_utils_.global_frame, feedback_utils_.robot_frame,
       feedback_utils_.transform_tolerance))
   {
-    bt_action_server_->setInternalError(ActionT::Result::POSE_NOT_AVAILABLE,
+    bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
       "Initial robot pose is not available.");
     return false;
   }
@@ -234,7 +235,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
         goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
         feedback_utils_.transform_tolerance))
     {
-      bt_action_server_->setInternalError(ActionT::Result::GOAL_TRANSFORMATION_ERROR,
+      bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
         "Failed to transform a goal pose (" + std::to_string(i) + ") provided with frame_id '" +
         goal_pose.header.frame_id +
         "' to the global frame '" +

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -44,8 +44,6 @@ NavigateToPoseNavigator::configure(
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;
 
-  bt_action_server_->resetInternalError();
-
   self_client_ = rclcpp_action::create_client<ActionT>(node, getName());
 
   goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
@@ -103,13 +101,15 @@ NavigateToPoseNavigator::goalCompleted(
   typename ActionT::Result::SharedPtr result,
   const nav2_behavior_tree::BtStatus /*final_bt_status*/)
 {
-  if (result->error_code == 0 && bt_action_server_->setResultToInternalError(result)) {
-    RCLCPP_WARN(logger_,
-      "NavigateToPoseNavigator::goalCompleted, set result to internal error %d:%s.",
-      result->error_code,
-      result->error_msg.c_str());
+  if (result->error_code == 0) {
+    if (bt_action_server_->populateInternalError(result)) {
+      RCLCPP_WARN(logger_,
+        "NavigateToPoseNavigator::goalCompleted, internal error %d:%s.",
+        result->error_code,
+        result->error_msg.c_str());
+    }
   } else {
-    RCLCPP_INFO(logger_, "NavigateThroughPosesNavigator::goalCompleted result %d:%s.",
+    RCLCPP_INFO(logger_, "NavigateThroughPosesNavigator::goalCompleted error %d:%s.",
       result->error_code,
       result->error_msg.c_str());
   }
@@ -225,7 +225,7 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
       feedback_utils_.global_frame, feedback_utils_.robot_frame,
       feedback_utils_.transform_tolerance))
   {
-    bt_action_server_->setInternalError(ActionT::Result::POSE_NOT_AVAILABLE,
+    bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
       "Initial robot pose is not available.");
     return false;
   }
@@ -235,7 +235,7 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
       goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
       feedback_utils_.transform_tolerance))
   {
-    bt_action_server_->setInternalError(ActionT::Result::GOAL_TRANSFORMATION_ERROR,
+    bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
       "Failed to transform a goal pose provided with frame_id '" +
       goal->pose.header.frame_id +
       "' to the global frame '" +

nav2_msgs/action/FollowWaypoints.action

@@ -10,7 +10,7 @@ geometry_msgs/PoseStamped[] poses
 uint16 NONE=0
 uint16 UNKNOWN=600
 uint16 TASK_EXECUTOR_FAILED=601
-uint16 NO_WAYPOINTS_GIVEN=602
+uint16 NO_VALID_WAYPOINTS=602
 uint16 STOP_ON_MISSED_WAYPOINT=603
 
 MissedWaypoint[] missed_waypoints

nav2_msgs/action/NavigateThroughPoses.action

@@ -8,10 +8,9 @@ string behavior_tree
 # Error codes
 # Note: The expected priority order of the errors should match the message order
 uint16 NONE=0
-uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=901
-uint16 GOAL_TRANSFORMATION_ERROR=902
-uint16 POSE_NOT_AVAILABLE=912
-uint16 UNKNOWN=999
+uint16 UNKNOWN=9910
+uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9911
+uint16 TF_ERROR=9912
 
 uint16 error_code
 string error_msg

nav2_msgs/action/NavigateToPose.action

@@ -7,10 +7,9 @@ string behavior_tree
 # Error codes
 # Note: The expected priority order of the errors should match the message order
 uint16 NONE=0
-uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=901
-uint16 GOAL_TRANSFORMATION_ERROR=902
-uint16 POSE_NOT_AVAILABLE=912
-uint16 UNKNOWN=999
+uint16 UNKNOWN=9900
+uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9901
+uint16 TF_ERROR=9902
 
 uint16 error_code
 string error_msg

nav2_system_tests/src/system/nav2_system_params.yaml

@@ -52,8 +52,18 @@ bt_navigator:
     navigate_through_poses:
       plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
     error_code_name_prefixes:
+      - assisted_teleop
+      - backup
       - compute_path
+      - dock_robot
+      - drive_on_heading
       - follow_path
+      - nav_thru_poses
+      - nav_to_pose
+      - spin
+      - route
+      - undock_robot
+      - wait
 
 controller_server:
   ros__parameters:

nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py

@@ -282,7 +282,7 @@ def run_all_tests(robot_tester):
         result = robot_tester.runNavigateAction(
             goal_pose=pose_out_of_bounds,
             behavior_tree='behavior_tree_that_does_not_exist.xml',
-            expected_error_code=901,
+            expected_error_code=NavigateThroughPoses.Result.FAILED_TO_LOAD_BEHAVIOR_TREE,
             expected_error_msg=('Error loading XML file: behavior_tree_that_does_not_exist.xml. '
                                 'Navigation canceled.'))
 

nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -233,7 +233,7 @@ void WaypointFollower::followWaypointsHandler(
 
   if (poses.empty()) {
     result->error_code =
-      nav2_msgs::action::FollowWaypoints::Result::NO_WAYPOINTS_GIVEN;
+      nav2_msgs::action::FollowWaypoints::Result::NO_VALID_WAYPOINTS;
     result->error_msg =
       "Empty vector of waypoints passed to waypoint following "
       "action potentially due to conversation failure or empty request.";
@@ -266,7 +266,7 @@ void WaypointFollower::followWaypointsHandler(
       poses = getLatestGoalPoses<T>(action_server);
       if (poses.empty()) {
         result->error_code =
-          nav2_msgs::action::FollowWaypoints::Result::NO_WAYPOINTS_GIVEN;
+          nav2_msgs::action::FollowWaypoints::Result::NO_VALID_WAYPOINTS;
         result->error_msg =
           "Empty vector of Waypoints passed to waypoint following logic. "
           "Nothing to execute, returning with failure!";
@@ -345,6 +345,7 @@ void WaypointFollower::followWaypointsHandler(
         missedWaypoint.goal = poses[goal_index];
         missedWaypoint.error_code =
           nav2_msgs::action::FollowWaypoints::Result::TASK_EXECUTOR_FAILED;
+        missedWaypoint.error_msg = "Task execution failed";
         result->missed_waypoints.push_back(missedWaypoint);
       }
       // if task execution was failed and stop_on_failure_ is on , terminate action