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 a151bc62920..fb3f683b462 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 @@ -30,7 +30,6 @@ namespace nav2_behavior_tree class AssistedTeleopAction : public BtActionNode { using Action = nav2_msgs::action::AssistedTeleop; - using ActionGoal = Action::Goal; using ActionResult = Action::Result; public: 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 d94685e3444..0f0886299a1 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 @@ -29,7 +29,6 @@ namespace nav2_behavior_tree class BackUpAction : public BtActionNode { using Action = nav2_msgs::action::BackUp; - using ActionGoal = Action::Goal; using ActionResult = Action::Result; public: 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 0b5f3b0972e..fe5d2e9dd25 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 @@ -34,7 +34,6 @@ class ComputePathThroughPosesAction { using Action = nav2_msgs::action::ComputePathThroughPoses; using ActionResult = Action::Result; - using ActionGoal = Action::Goal; public: /** 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 829fd044282..07e9a4a2f2f 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 @@ -31,7 +31,6 @@ class ComputePathToPoseAction : public BtActionNode { using Action = nav2_msgs::action::DriveOnHeading; - using ActionGoal = Action::Goal; - using ActionResult = Action::Goal; + using ActionResult = Action::Result; public: /** 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 7a1762798a0..c05c2c0d59f 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 @@ -31,7 +31,6 @@ class FollowPathAction : public BtActionNode { using Action = nav2_msgs::action::FollowPath; using ActionResult = Action::Result; - using ActionGoal = Action::Goal; public: /** 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 4b9ac96f7ae..44ad055ea24 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 @@ -33,7 +33,6 @@ class NavigateThroughPosesAction : public BtActionNode { using Action = nav2_msgs::action::Spin; using ActionResult = Action::Result; - using ActionGoal = Action::Goal; public: /** diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.hpp index 6c5b4f3982e..bdd979f7889 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.hpp @@ -26,7 +26,7 @@ namespace nav2_behavior_tree class WouldAControllerRecoveryHelp : public AreErrorCodesPresent { using Action = nav2_msgs::action::FollowPath; - using ActionGoal = Action::Goal; + using ActionResult = Action::Result; public: WouldAControllerRecoveryHelp( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp index 4c37893bc4b..e53d6e4c351 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp @@ -26,7 +26,7 @@ namespace nav2_behavior_tree class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent { using Action = nav2_msgs::action::ComputePathToPose; - using ActionGoal = Action::Goal; + using ActionResult = Action::Result; public: WouldAPlannerRecoveryHelp( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.hpp index a13a6c76eb9..8cdbcd156dc 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.hpp @@ -27,7 +27,7 @@ namespace nav2_behavior_tree class WouldASmootherRecoveryHelp : public AreErrorCodesPresent { using Action = nav2_msgs::action::SmoothPath; - using ActionGoal = Action::Goal; + using ActionResult = Action::Result; public: WouldASmootherRecoveryHelp( diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 1f63a281c3e..1d2bad9f376 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -43,7 +43,7 @@ void AssistedTeleopAction::on_tick() BT::NodeStatus AssistedTeleopAction::on_success() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -55,7 +55,7 @@ BT::NodeStatus AssistedTeleopAction::on_aborted() BT::NodeStatus AssistedTeleopAction::on_cancelled() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 9013a1244c1..cb15fc93336 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -48,7 +48,7 @@ void BackUpAction::on_tick() BT::NodeStatus BackUpAction::on_success() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -60,7 +60,7 @@ BT::NodeStatus BackUpAction::on_aborted() BT::NodeStatus BackUpAction::on_cancelled() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 5fede84f2cf..8bfffed8434 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 @@ -42,7 +42,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -59,7 +59,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 0619a41ee06..c4b93afa854 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 @@ -41,7 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -58,7 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 73036ccddd6..be062357b16 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -43,7 +43,7 @@ DriveOnHeadingAction::DriveOnHeadingAction( BT::NodeStatus DriveOnHeadingAction::on_success() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -55,7 +55,7 @@ BT::NodeStatus DriveOnHeadingAction::on_aborted() BT::NodeStatus DriveOnHeadingAction::on_cancelled() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 d6175a59084..00acf5c3495 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -38,7 +38,7 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -51,7 +51,7 @@ BT::NodeStatus FollowPathAction::on_aborted() BT::NodeStatus FollowPathAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 5e3e65f424b..57db8942481 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -41,7 +41,7 @@ void NavigateThroughPosesAction::on_tick() BT::NodeStatus NavigateThroughPosesAction::on_success() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -54,7 +54,7 @@ BT::NodeStatus NavigateThroughPosesAction::on_aborted() BT::NodeStatus NavigateThroughPosesAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 dabc576fccd..8022697e0e8 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -40,7 +40,7 @@ void NavigateToPoseAction::on_tick() BT::NodeStatus NavigateToPoseAction::on_success() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -53,7 +53,7 @@ BT::NodeStatus NavigateToPoseAction::on_aborted() BT::NodeStatus NavigateToPoseAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 b3291b73ccc..da547b15876 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -45,7 +45,7 @@ BT::NodeStatus SmoothPathAction::on_success() setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds()); setOutput("was_completed", result_.result->was_completed); // Set empty error code, action was successful - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -58,7 +58,7 @@ BT::NodeStatus SmoothPathAction::on_aborted() BT::NodeStatus SmoothPathAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); 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 dd203499502..78fa1311752 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -43,7 +43,7 @@ void SpinAction::on_tick() BT::NodeStatus SpinAction::on_success() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } @@ -55,7 +55,7 @@ BT::NodeStatus SpinAction::on_aborted() BT::NodeStatus SpinAction::on_cancelled() { - setOutput("error_code_id", ActionGoal::NONE); + setOutput("error_code_id", ActionResult::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp index f299f4b7b42..f8c62064f6c 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp @@ -24,10 +24,10 @@ WouldAControllerRecoveryHelp::WouldAControllerRecoveryHelp( : AreErrorCodesPresent(condition_name, conf) { error_codes_to_check_ = { - ActionGoal::UNKNOWN, - ActionGoal::PATIENCE_EXCEEDED, - ActionGoal::FAILED_TO_MAKE_PROGRESS, - ActionGoal::NO_VALID_CONTROL + ActionResult::UNKNOWN, + ActionResult::PATIENCE_EXCEEDED, + ActionResult::FAILED_TO_MAKE_PROGRESS, + ActionResult::NO_VALID_CONTROL }; } diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp index 2420c1ea025..6a8d3f33cad 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp @@ -24,9 +24,9 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp( : AreErrorCodesPresent(condition_name, conf) { error_codes_to_check_ = { - ActionGoal::UNKNOWN, - ActionGoal::NO_VALID_PATH, - ActionGoal::TIMEOUT + ActionResult::UNKNOWN, + ActionResult::NO_VALID_PATH, + ActionResult::TIMEOUT }; } diff --git a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp index 7bcf01f0915..fb903e1cbfc 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp @@ -24,10 +24,10 @@ WouldASmootherRecoveryHelp::WouldASmootherRecoveryHelp( : AreErrorCodesPresent(condition_name, conf) { error_codes_to_check_ = { - ActionGoal::UNKNOWN, - ActionGoal::TIMEOUT, - ActionGoal::FAILED_TO_SMOOTH_PATH, - ActionGoal::SMOOTHED_PATH_IN_COLLISION + ActionResult::UNKNOWN, + ActionResult::TIMEOUT, + ActionResult::FAILED_TO_SMOOTH_PATH, + ActionResult::SMOOTHED_PATH_IN_COLLISION }; } diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp index 993f3b4147d..d1716397df0 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp @@ -24,11 +24,11 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF { public: using Action = nav2_msgs::action::FollowPath; - using ActionGoal = Action::Goal; + using ActionResult = Action::Result; void SetUp() { - uint16_t error_code = ActionGoal::NONE; - std::set error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT + uint16_t error_code = ActionResult::NONE; + std::set error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT config_->blackboard->set("error_code", error_code); config_->blackboard->set("error_codes_to_check", error_codes_to_check); @@ -59,8 +59,8 @@ std::shared_ptr AreErrorCodesPresentFixture::tree_ = nullptr; TEST_F(AreErrorCodesPresentFixture, test_condition) { std::map error_to_status_map = { - {ActionGoal::NONE, BT::NodeStatus::FAILURE}, - {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS}, + {ActionResult::NONE, BT::NodeStatus::FAILURE}, + {ActionResult::UNKNOWN, BT::NodeStatus::SUCCESS}, }; for (const auto & error_to_status : error_to_status_map) { diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp index ec9c6bd0449..d93249879d5 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp @@ -24,10 +24,10 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT { public: using Action = nav2_msgs::action::FollowPath; - using ActionGoal = Action::Goal; + using ActionResult = Action::Result; void SetUp() { - uint16_t error_code = ActionGoal::NONE; + uint16_t error_code = ActionResult::NONE; config_->blackboard->set("error_code", error_code); std::string xml_txt = @@ -57,14 +57,14 @@ std::shared_ptr WouldAControllerRecoveryHelpFixture::tree_ = nullptr; TEST_F(WouldAControllerRecoveryHelpFixture, test_condition) { std::map error_to_status_map = { - {ActionGoal::NONE, BT::NodeStatus::FAILURE}, - {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS}, - {ActionGoal::INVALID_CONTROLLER, BT::NodeStatus::FAILURE}, - {ActionGoal::TF_ERROR, BT::NodeStatus::FAILURE}, - {ActionGoal::INVALID_PATH, BT::NodeStatus::FAILURE}, - {ActionGoal::PATIENCE_EXCEEDED, BT::NodeStatus::SUCCESS}, - {ActionGoal::FAILED_TO_MAKE_PROGRESS, BT::NodeStatus::SUCCESS}, - {ActionGoal::NO_VALID_CONTROL, BT::NodeStatus::SUCCESS}, + {ActionResult::NONE, BT::NodeStatus::FAILURE}, + {ActionResult::UNKNOWN, BT::NodeStatus::SUCCESS}, + {ActionResult::INVALID_CONTROLLER, BT::NodeStatus::FAILURE}, + {ActionResult::TF_ERROR, BT::NodeStatus::FAILURE}, + {ActionResult::INVALID_PATH, BT::NodeStatus::FAILURE}, + {ActionResult::PATIENCE_EXCEEDED, BT::NodeStatus::SUCCESS}, + {ActionResult::FAILED_TO_MAKE_PROGRESS, BT::NodeStatus::SUCCESS}, + {ActionResult::NO_VALID_CONTROL, BT::NodeStatus::SUCCESS}, }; for (const auto & error_to_status : error_to_status_map) { diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp index a4ccbb07def..4dc0f8bb5cb 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp @@ -24,10 +24,10 @@ class WouldAPlannerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTree { public: using Action = nav2_msgs::action::ComputePathToPose; - using ActionGoal = Action::Goal; + using ActionResult = Action::Result; void SetUp() { - uint16_t error_code = ActionGoal::NONE; + uint16_t error_code = ActionResult::NONE; config_->blackboard->set("error_code", error_code); std::string xml_txt = @@ -57,9 +57,9 @@ std::shared_ptr WouldAPlannerRecoveryHelpFixture::tree_ = nullptr; TEST_F(WouldAPlannerRecoveryHelpFixture, test_condition) { std::map error_to_status_map = { - {ActionGoal::NONE, BT::NodeStatus::FAILURE}, - {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS}, - {ActionGoal::NO_VALID_PATH, BT::NodeStatus::SUCCESS}, + {ActionResult::NONE, BT::NodeStatus::FAILURE}, + {ActionResult::UNKNOWN, BT::NodeStatus::SUCCESS}, + {ActionResult::NO_VALID_PATH, BT::NodeStatus::SUCCESS}, }; for (const auto & error_to_status : error_to_status_map) { diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp index cd697773ba0..337d5133c38 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp @@ -24,7 +24,7 @@ class WouldASmootherRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTre { public: using Action = nav2_msgs::action::SmoothPath; - using ActionGoal = Action::Goal; + using ActionGoal = Action::Result; void SetUp() { uint16_t error_code = ActionGoal::NONE; diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index dee7f7232f1..8c2e85f342b 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -68,7 +68,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptrtime_allowance; end_time_ = steady_clock_.now() + command_time_allowance_; - return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionGoal::NONE}; + return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE}; } void AssistedTeleop::onActionCompletion() @@ -89,13 +89,13 @@ ResultStatus AssistedTeleop::onCycleUpdate() logger_, "Exceeded time allowance before reaching the " << behavior_name_.c_str() << "goal - Exiting " << behavior_name_.c_str()); - return ResultStatus{Status::FAILED, AssistedTeleopActionGoal::TIMEOUT}; + return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TIMEOUT}; } // user states that teleop was successful if (preempt_teleop_) { stopRobot(); - return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionGoal::NONE}; + return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE}; } geometry_msgs::msg::PoseStamped current_pose; @@ -107,7 +107,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() logger_, "Current robot pose is not available for " << behavior_name_.c_str()); - return ResultStatus{Status::FAILED, AssistedTeleopActionGoal::TF_ERROR}; + return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TF_ERROR}; } geometry_msgs::msg::Pose2D projected_pose; @@ -148,7 +148,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() } vel_pub_->publish(std::move(scaled_twist)); - return ResultStatus{Status::RUNNING, AssistedTeleopActionGoal::NONE}; + return ResultStatus{Status::RUNNING, AssistedTeleopActionResult::NONE}; } geometry_msgs::msg::Pose2D AssistedTeleop::projectPose( diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/plugins/assisted_teleop.hpp index 9b0423b96ce..10fbec6c820 100644 --- a/nav2_behaviors/plugins/assisted_teleop.hpp +++ b/nav2_behaviors/plugins/assisted_teleop.hpp @@ -38,6 +38,7 @@ class AssistedTeleop : public TimedBehavior public: using AssistedTeleopActionGoal = AssistedTeleopAction::Goal; + using AssistedTeleopActionResult = AssistedTeleopAction::Result; AssistedTeleop(); /** diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index a72a0c3830e..aed843d75c6 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -23,7 +23,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr comma RCLCPP_INFO( logger_, "Backing up in Y and Z not supported, will only move in X."); - return ResultStatus{Status::FAILED, BackUpActionGoal::INVALID_INPUT}; + return ResultStatus{Status::FAILED, BackUpActionResult::INVALID_INPUT}; } // Silently ensure that both the speed and direction are negative. @@ -38,10 +38,10 @@ ResultStatus BackUp::onRun(const std::shared_ptr comma transform_tolerance_)) { RCLCPP_ERROR(logger_, "Initial robot pose is not available."); - return ResultStatus{Status::FAILED, BackUpActionGoal::TF_ERROR}; + return ResultStatus{Status::FAILED, BackUpActionResult::TF_ERROR}; } - return ResultStatus{Status::SUCCEEDED, BackUpActionGoal::NONE}; + return ResultStatus{Status::SUCCEEDED, BackUpActionResult::NONE}; } } // namespace nav2_behaviors diff --git a/nav2_behaviors/plugins/back_up.hpp b/nav2_behaviors/plugins/back_up.hpp index 3715297554d..7c78abf7b83 100644 --- a/nav2_behaviors/plugins/back_up.hpp +++ b/nav2_behaviors/plugins/back_up.hpp @@ -29,9 +29,11 @@ class BackUp : public DriveOnHeading { public: using BackUpActionGoal = BackUpAction::Goal; + using BackUpActionResult = BackUpAction::Result; ResultStatus onRun(const std::shared_ptr command) override; }; -} + +} // namespace nav2_behaviors #endif // NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_ diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp index 892ed2d9eec..cd32a9f48c4 100644 --- a/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -63,13 +63,13 @@ class DriveOnHeading : public TimedBehavior RCLCPP_INFO( this->logger_, "DrivingOnHeading in Y and Z not supported, will only move in X."); - return ResultStatus{Status::FAILED, ActionT::Goal::INVALID_INPUT}; + return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT}; } // Ensure that both the speed and direction have the same sign if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) { RCLCPP_ERROR(this->logger_, "Speed and command sign did not match"); - return ResultStatus{Status::FAILED, ActionT::Goal::INVALID_INPUT}; + return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT}; } command_x_ = command->target.x; @@ -83,10 +83,10 @@ class DriveOnHeading : public TimedBehavior this->transform_tolerance_)) { RCLCPP_ERROR(this->logger_, "Initial robot pose is not available."); - return ResultStatus{Status::FAILED, ActionT::Goal::TF_ERROR}; + return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR}; } - return ResultStatus{Status::SUCCEEDED, ActionT::Goal::NONE}; + return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE}; } /** @@ -101,7 +101,7 @@ class DriveOnHeading : public TimedBehavior RCLCPP_WARN( this->logger_, "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading"); - return ResultStatus{Status::FAILED, ActionT::Goal::NONE}; + return ResultStatus{Status::FAILED, ActionT::Result::NONE}; } geometry_msgs::msg::PoseStamped current_pose; @@ -110,7 +110,7 @@ class DriveOnHeading : public TimedBehavior this->transform_tolerance_)) { RCLCPP_ERROR(this->logger_, "Current robot pose is not available."); - return ResultStatus{Status::FAILED, ActionT::Goal::TF_ERROR}; + return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR}; } double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x; @@ -122,7 +122,7 @@ class DriveOnHeading : public TimedBehavior if (distance >= std::fabs(command_x_)) { this->stopRobot(); - return ResultStatus{Status::SUCCEEDED, ActionT::Goal::NONE}; + return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE}; } auto cmd_vel = std::make_unique(); @@ -138,12 +138,12 @@ class DriveOnHeading : public TimedBehavior if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { this->stopRobot(); RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading"); - return ResultStatus{Status::FAILED, ActionT::Goal::COLLISION_AHEAD}; + return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD}; } this->vel_pub_->publish(std::move(cmd_vel)); - return ResultStatus{Status::RUNNING, ActionT::Goal::NONE}; + return ResultStatus{Status::RUNNING, ActionT::Result::NONE}; } /** diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index ad10fc1d0c0..714483c85cd 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -79,7 +79,7 @@ ResultStatus Spin::onRun(const std::shared_ptr command) transform_tolerance_)) { RCLCPP_ERROR(logger_, "Current robot pose is not available."); - return ResultStatus{Status::FAILED, SpinActionGoal::TF_ERROR}; + return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR}; } prev_yaw_ = tf2::getYaw(current_pose.pose.orientation); @@ -93,7 +93,7 @@ ResultStatus Spin::onRun(const std::shared_ptr command) command_time_allowance_ = command->time_allowance; end_time_ = steady_clock_.now() + command_time_allowance_; - return ResultStatus{Status::SUCCEEDED, SpinActionGoal::NONE}; + return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE}; } ResultStatus Spin::onCycleUpdate() @@ -104,7 +104,7 @@ ResultStatus Spin::onCycleUpdate() RCLCPP_WARN( logger_, "Exceeded time allowance before reaching the Spin goal - Exiting Spin"); - return ResultStatus{Status::FAILED, SpinActionGoal::TIMEOUT}; + return ResultStatus{Status::FAILED, SpinActionResult::TIMEOUT}; } geometry_msgs::msg::PoseStamped current_pose; @@ -113,7 +113,7 @@ ResultStatus Spin::onCycleUpdate() transform_tolerance_)) { RCLCPP_ERROR(logger_, "Current robot pose is not available."); - return ResultStatus{Status::FAILED, SpinActionGoal::TF_ERROR}; + return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR}; } const double current_yaw = tf2::getYaw(current_pose.pose.orientation); @@ -132,7 +132,7 @@ ResultStatus Spin::onCycleUpdate() double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_); if (remaining_yaw < 1e-6) { stopRobot(); - return ResultStatus{Status::SUCCEEDED, SpinActionGoal::NONE}; + return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE}; } double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw); @@ -149,12 +149,12 @@ ResultStatus Spin::onCycleUpdate() if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin"); - return ResultStatus{Status::FAILED, SpinActionGoal::COLLISION_AHEAD}; + return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD}; } vel_pub_->publish(std::move(cmd_vel)); - return ResultStatus{Status::RUNNING, SpinActionGoal::NONE}; + return ResultStatus{Status::RUNNING, SpinActionResult::NONE}; } bool Spin::isCollisionFree( diff --git a/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/plugins/spin.hpp index e9388e359c6..01358ddbd33 100644 --- a/nav2_behaviors/plugins/spin.hpp +++ b/nav2_behaviors/plugins/spin.hpp @@ -37,6 +37,7 @@ class Spin : public TimedBehavior public: using SpinActionGoal = SpinAction::Goal; + using SpinActionResult = SpinAction::Result; /** * @brief A constructor for nav2_behaviors::Spin diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d27cb6692d2..c76f9f86ccd 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -491,56 +491,56 @@ void ControllerServer::computeControl() RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::INVALID_CONTROLLER; + result->error_code = Action::Result::INVALID_CONTROLLER; action_server_->terminate_current(result); return; } catch (nav2_core::ControllerTFError & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::TF_ERROR; + result->error_code = Action::Result::TF_ERROR; action_server_->terminate_current(result); return; } catch (nav2_core::NoValidControl & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::NO_VALID_CONTROL; + result->error_code = Action::Result::NO_VALID_CONTROL; action_server_->terminate_current(result); return; } catch (nav2_core::FailedToMakeProgress & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::FAILED_TO_MAKE_PROGRESS; + result->error_code = Action::Result::FAILED_TO_MAKE_PROGRESS; action_server_->terminate_current(result); return; } catch (nav2_core::PatienceExceeded & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::PATIENCE_EXCEEDED; + result->error_code = Action::Result::PATIENCE_EXCEEDED; action_server_->terminate_current(result); return; } catch (nav2_core::InvalidPath & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::INVALID_PATH; + result->error_code = Action::Result::INVALID_PATH; action_server_->terminate_current(result); return; } catch (nav2_core::ControllerException & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::UNKNOWN; + result->error_code = Action::Result::UNKNOWN; action_server_->terminate_current(result); return; } catch (std::exception & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); std::shared_ptr result = std::make_shared(); - result->error_code = Action::Goal::UNKNOWN; + result->error_code = Action::Result::UNKNOWN; action_server_->terminate_current(result); return; } diff --git a/nav2_msgs/action/AssistedTeleop.action b/nav2_msgs/action/AssistedTeleop.action index 7507c46cf5a..8737bdd0cde 100644 --- a/nav2_msgs/action/AssistedTeleop.action +++ b/nav2_msgs/action/AssistedTeleop.action @@ -1,3 +1,8 @@ +#goal definition +builtin_interfaces/Duration time_allowance +--- +#result definition + # Error codes # Note: The expected priority order of the error should match the message order uint16 NONE=0 @@ -5,10 +10,6 @@ uint16 UNKNOWN=730 uint16 TIMEOUT=731 uint16 TF_ERROR=732 -#goal definition -builtin_interfaces/Duration time_allowance ---- -#result definition builtin_interfaces/Duration total_elapsed_time uint16 error_code --- diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 298e8c56ba1..28711bfa5d8 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -1,3 +1,11 @@ + +#goal definition +geometry_msgs/Point target +float32 speed +builtin_interfaces/Duration time_allowance +--- +#result definition + # Error codes # Note: The expected priority order of the error should match the message order uint16 NONE=0 @@ -7,12 +15,6 @@ uint16 TF_ERROR=712 uint16 INVALID_INPUT=713 uint16 COLLISION_AHEAD=714 -#goal definition -geometry_msgs/Point target -float32 speed -builtin_interfaces/Duration time_allowance ---- -#result definition builtin_interfaces/Duration total_elapsed_time uint16 error_code --- diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 38c7f70a5da..1dc0cbd2c12 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,3 +1,11 @@ +#goal definition +geometry_msgs/PoseStamped[] goals +geometry_msgs/PoseStamped start +string planner_id +bool use_start # If false, use current robot pose as path start, if true, use start above instead +--- +#result definition + # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 @@ -12,13 +20,6 @@ uint16 TIMEOUT=3007 uint16 NO_VALID_PATH=308 uint16 NO_VIAPOINTS_GIVEN=309 -#goal definition -geometry_msgs/PoseStamped[] goals -geometry_msgs/PoseStamped start -string planner_id -bool use_start # If false, use current robot pose as path start, if true, use start above instead ---- -#result definition nav_msgs/Path path builtin_interfaces/Duration planning_time uint16 error_code diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 920bcf5ff24..ef8b20d3077 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,3 +1,11 @@ +#goal definition +geometry_msgs/PoseStamped goal +geometry_msgs/PoseStamped start +string planner_id +bool use_start # If false, use current robot pose as path start, if true, use start above instead +--- +#result definition + # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 @@ -11,13 +19,6 @@ uint16 GOAL_OCCUPIED=206 uint16 TIMEOUT=207 uint16 NO_VALID_PATH=208 -#goal definition -geometry_msgs/PoseStamped goal -geometry_msgs/PoseStamped start -string planner_id -bool use_start # If false, use current robot pose as path start, if true, use start above instead ---- -#result definition nav_msgs/Path path builtin_interfaces/Duration planning_time uint16 error_code diff --git a/nav2_msgs/action/DriveOnHeading.action b/nav2_msgs/action/DriveOnHeading.action index 529d88d61b6..67d2badeef9 100644 --- a/nav2_msgs/action/DriveOnHeading.action +++ b/nav2_msgs/action/DriveOnHeading.action @@ -1,3 +1,11 @@ + +#goal definition +geometry_msgs/Point target +float32 speed +builtin_interfaces/Duration time_allowance +--- +#result definition + # Error codes # Note: The expected priority order of the error should match the message order uint16 NONE=0 @@ -7,12 +15,6 @@ uint16 TF_ERROR=722 uint16 COLLISION_AHEAD=723 uint16 INVALID_INPUT=724 -#goal definition -geometry_msgs/Point target -float32 speed -builtin_interfaces/Duration time_allowance ---- -#result definition builtin_interfaces/Duration total_elapsed_time uint16 error_code --- diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 8587e56294e..654e43192ee 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,3 +1,11 @@ +#goal definition +nav_msgs/Path path +string controller_id +string goal_checker_id +string progress_checker_id +--- +#result definition + # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 @@ -9,13 +17,6 @@ uint16 PATIENCE_EXCEEDED=104 uint16 FAILED_TO_MAKE_PROGRESS=105 uint16 NO_VALID_CONTROL=106 -#goal definition -nav_msgs/Path path -string controller_id -string goal_checker_id -string progress_checker_id ---- -#result definition std_msgs/Empty result uint16 error_code --- diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index 1c5a38e8b1f..a8706cd59cf 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,15 +1,16 @@ -# Error codes -# Note: The expected priority order of the errors should match the message order -uint16 NONE=0 -uint16 UNKNOWN=600 -uint16 TASK_EXECUTOR_FAILED=601 - #goal definition uint32 number_of_loops uint32 goal_index 0 geometry_msgs/PoseStamped[] poses --- #result definition + +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 + MissedWaypoint[] missed_waypoints --- #feedback definition diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index a6bb2b93887..efc47673064 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,12 +1,14 @@ -# Error codes -# Note: The expected priority order of the errors should match the message order -uint16 NONE=0 #goal definition geometry_msgs/PoseStamped[] poses string behavior_tree --- #result definition + +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 9fcf8e65fdb..c3c25c9fee7 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -1,12 +1,13 @@ -# Error codes -# Note: The expected priority order of the errors should match the message order -uint16 NONE=0 - #goal definition geometry_msgs/PoseStamped pose string behavior_tree --- #result definition + +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/SmoothPath.action b/nav2_msgs/action/SmoothPath.action index d7443fce5aa..8b04d1d1042 100644 --- a/nav2_msgs/action/SmoothPath.action +++ b/nav2_msgs/action/SmoothPath.action @@ -1,3 +1,11 @@ +#goal definition +nav_msgs/Path path +string smoother_id +builtin_interfaces/Duration max_smoothing_duration +bool check_for_collisions +--- +#result definition + # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 @@ -8,13 +16,6 @@ uint16 SMOOTHED_PATH_IN_COLLISION=503 uint16 FAILED_TO_SMOOTH_PATH=504 uint16 INVALID_PATH=505 -#goal definition -nav_msgs/Path path -string smoother_id -builtin_interfaces/Duration max_smoothing_duration -bool check_for_collisions ---- -#result definition nav_msgs/Path path builtin_interfaces/Duration smoothing_duration bool was_completed diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index 036cd1b8f50..945a16d3d66 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -1,3 +1,9 @@ +#goal definition +float32 target_yaw +builtin_interfaces/Duration time_allowance +--- +#result definition + # Error codes # Note: The expected priority order of the error should match the message order uint16 NONE=0 @@ -6,11 +12,6 @@ uint16 TIMEOUT=701 uint16 TF_ERROR=702 uint16 COLLISION_AHEAD=703 -#goal definition -float32 target_yaw -builtin_interfaces/Duration time_allowance ---- -#result definition builtin_interfaces/Duration total_elapsed_time uint16 error_code --- diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index fee711f974a..c54b9ae829b 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -107,9 +107,9 @@ class PlannerServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; using ActionToPose = nav2_msgs::action::ComputePathToPose; - using ActionToPoseGoal = ActionToPose::Goal; + using ActionToPoseResult = ActionToPose::Result; using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses; - using ActionThroughPosesGoal = ActionThroughPoses::Goal; + using ActionThroughPosesResult = ActionThroughPoses::Result; using ActionServerToPose = nav2_util::SimpleActionServer; using ActionServerThroughPoses = nav2_util::SimpleActionServer; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e8c63fcd1e9..eb7396d34fd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -436,43 +436,43 @@ void PlannerServer::computePlanThroughPoses() action_server_poses_->succeeded_current(result); } catch (nav2_core::InvalidPlanner & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::INVALID_PLANNER; + result->error_code = ActionThroughPosesResult::INVALID_PLANNER; action_server_poses_->terminate_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::START_OCCUPIED; + result->error_code = ActionThroughPosesResult::START_OCCUPIED; action_server_poses_->terminate_current(result); } catch (nav2_core::GoalOccupied & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::GOAL_OCCUPIED; + result->error_code = ActionThroughPosesResult::GOAL_OCCUPIED; action_server_poses_->terminate_current(result); } catch (nav2_core::NoValidPathCouldBeFound & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::NO_VALID_PATH; + result->error_code = ActionThroughPosesResult::NO_VALID_PATH; action_server_poses_->terminate_current(result); } catch (nav2_core::PlannerTimedOut & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::TIMEOUT; + result->error_code = ActionThroughPosesResult::TIMEOUT; action_server_poses_->terminate_current(result); } catch (nav2_core::StartOutsideMapBounds & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::START_OUTSIDE_MAP; + result->error_code = ActionThroughPosesResult::START_OUTSIDE_MAP; action_server_poses_->terminate_current(result); } catch (nav2_core::GoalOutsideMapBounds & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::GOAL_OUTSIDE_MAP; + result->error_code = ActionThroughPosesResult::GOAL_OUTSIDE_MAP; action_server_poses_->terminate_current(result); } catch (nav2_core::PlannerTFError & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::TF_ERROR; + result->error_code = ActionThroughPosesResult::TF_ERROR; action_server_poses_->terminate_current(result); } catch (nav2_core::NoViapointsGiven & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::NO_VIAPOINTS_GIVEN; + result->error_code = ActionThroughPosesResult::NO_VIAPOINTS_GIVEN; action_server_poses_->terminate_current(result); } catch (std::exception & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); - result->error_code = ActionThroughPosesGoal::UNKNOWN; + result->error_code = ActionThroughPosesResult::UNKNOWN; action_server_poses_->terminate_current(result); } } @@ -531,39 +531,39 @@ PlannerServer::computePlan() action_server_pose_->succeeded_current(result); } catch (nav2_core::InvalidPlanner & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::INVALID_PLANNER; + result->error_code = ActionToPoseResult::INVALID_PLANNER; action_server_pose_->terminate_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::START_OCCUPIED; + result->error_code = ActionToPoseResult::START_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::GOAL_OCCUPIED; + result->error_code = ActionToPoseResult::GOAL_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::NoValidPathCouldBeFound & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::NO_VALID_PATH; + result->error_code = ActionToPoseResult::NO_VALID_PATH; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTimedOut & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::TIMEOUT; + result->error_code = ActionToPoseResult::TIMEOUT; action_server_pose_->terminate_current(result); } catch (nav2_core::StartOutsideMapBounds & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::START_OUTSIDE_MAP; + result->error_code = ActionToPoseResult::START_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOutsideMapBounds & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::GOAL_OUTSIDE_MAP; + result->error_code = ActionToPoseResult::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTFError & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::TF_ERROR; + result->error_code = ActionToPoseResult::TF_ERROR; action_server_pose_->terminate_current(result); } catch (std::exception & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = ActionToPoseGoal::UNKNOWN; + result->error_code = ActionToPoseResult::UNKNOWN; action_server_pose_->terminate_current(result); } } diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index b52ebf65219..35eaadb8614 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -114,7 +114,7 @@ class SmootherServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::SmoothPath; - using ActionGoal = Action::Goal; + using ActionResult = Action::Result; using ActionServer = nav2_util::SimpleActionServer; /** diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 503521f3e6f..522d3618304 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -321,37 +321,37 @@ void SmootherServer::smoothPlan() action_server_->succeeded_current(result); } catch (nav2_core::InvalidSmoother & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - result->error_code = ActionGoal::INVALID_SMOOTHER; + result->error_code = ActionResult::INVALID_SMOOTHER; action_server_->terminate_current(result); return; } catch (nav2_core::SmootherTimedOut & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - result->error_code = ActionGoal::TIMEOUT; + result->error_code = ActionResult::TIMEOUT; action_server_->terminate_current(result); return; } catch (nav2_core::SmoothedPathInCollision & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - result->error_code = ActionGoal::SMOOTHED_PATH_IN_COLLISION; + result->error_code = ActionResult::SMOOTHED_PATH_IN_COLLISION; action_server_->terminate_current(result); return; } catch (nav2_core::FailedToSmoothPath & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - result->error_code = ActionGoal::FAILED_TO_SMOOTH_PATH; + result->error_code = ActionResult::FAILED_TO_SMOOTH_PATH; action_server_->terminate_current(result); return; } catch (nav2_core::InvalidPath & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - result->error_code = ActionGoal::INVALID_PATH; + result->error_code = ActionResult::INVALID_PATH; action_server_->terminate_current(result); return; } catch (nav2_core::SmootherException & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - result->error_code = ActionGoal::UNKNOWN; + result->error_code = ActionResult::UNKNOWN; action_server_->terminate_current(result); return; } catch (std::exception & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - result->error_code = ActionGoal::UNKNOWN; + result->error_code = ActionResult::UNKNOWN; action_server_->terminate_current(result); return; } diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp index a85134d4109..b407089b21f 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.hpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -74,7 +74,7 @@ class DummyComputePathToPoseActionServer std::shared_ptr & result) override { - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; + result->error_code = nav2_msgs::action::ComputePathToPose::Result::TIMEOUT; } private: @@ -92,7 +92,7 @@ class DummyFollowPathActionServer : public DummyActionServer & result) override { - result->error_code = nav2_msgs::action::FollowPath::Goal::NO_VALID_CONTROL; + result->error_code = nav2_msgs::action::FollowPath::Result::NO_VALID_CONTROL; } }; diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 57294873ec4..7a46f5fd812 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -54,13 +54,13 @@ def main(argv=sys.argv[1:]): navigator._waitForNodeToActivate('controller_server') follow_path = { - 'unknown': FollowPath.Goal().UNKNOWN, - 'invalid_controller': FollowPath.Goal().INVALID_CONTROLLER, - 'tf_error': FollowPath.Goal().TF_ERROR, - 'invalid_path': FollowPath.Goal().INVALID_PATH, - 'patience_exceeded': FollowPath.Goal().PATIENCE_EXCEEDED, - 'failed_to_make_progress': FollowPath.Goal().FAILED_TO_MAKE_PROGRESS, - 'no_valid_control': FollowPath.Goal().NO_VALID_CONTROL + 'unknown': FollowPath.Result().UNKNOWN, + 'invalid_controller': FollowPath.Result().INVALID_CONTROLLER, + 'tf_error': FollowPath.Result().TF_ERROR, + 'invalid_path': FollowPath.Result().INVALID_PATH, + 'patience_exceeded': FollowPath.Result().PATIENCE_EXCEEDED, + 'failed_to_make_progress': FollowPath.Result().FAILED_TO_MAKE_PROGRESS, + 'no_valid_control': FollowPath.Result().NO_VALID_CONTROL } for controller, error_code in follow_path.items(): @@ -88,15 +88,15 @@ def main(argv=sys.argv[1:]): navigator._waitForNodeToActivate('planner_server') compute_path_to_pose = { - 'unknown': ComputePathToPose.Goal().UNKNOWN, - 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, - 'tf_error': ComputePathToPose.Goal().TF_ERROR, - 'start_outside_map': ComputePathToPose.Goal().START_OUTSIDE_MAP, - 'goal_outside_map': ComputePathToPose.Goal().GOAL_OUTSIDE_MAP, - 'start_occupied': ComputePathToPose.Goal().START_OCCUPIED, - 'goal_occupied': ComputePathToPose.Goal().GOAL_OCCUPIED, - 'timeout': ComputePathToPose.Goal().TIMEOUT, - 'no_valid_path': ComputePathToPose.Goal().NO_VALID_PATH} + 'unknown': ComputePathToPose.Result().UNKNOWN, + 'invalid_planner': ComputePathToPose.Result().INVALID_PLANNER, + 'tf_error': ComputePathToPose.Result().TF_ERROR, + 'start_outside_map': ComputePathToPose.Result().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathToPose.Result().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathToPose.Result().START_OCCUPIED, + 'goal_occupied': ComputePathToPose.Result().GOAL_OCCUPIED, + 'timeout': ComputePathToPose.Result().TIMEOUT, + 'no_valid_path': ComputePathToPose.Result().NO_VALID_PATH} for planner, error_code in compute_path_to_pose.items(): result = navigator._getPathImpl(initial_pose, goal_pose, planner) @@ -107,16 +107,16 @@ def main(argv=sys.argv[1:]): goal_poses = [goal_pose, goal_pose1] compute_path_through_poses = { - 'unknown': ComputePathThroughPoses.Goal().UNKNOWN, - 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, - 'tf_error': ComputePathThroughPoses.Goal().TF_ERROR, - 'start_outside_map': ComputePathThroughPoses.Goal().START_OUTSIDE_MAP, - 'goal_outside_map': ComputePathThroughPoses.Goal().GOAL_OUTSIDE_MAP, - 'start_occupied': ComputePathThroughPoses.Goal().START_OCCUPIED, - 'goal_occupied': ComputePathThroughPoses.Goal().GOAL_OCCUPIED, - 'timeout': ComputePathThroughPoses.Goal().TIMEOUT, - 'no_valid_path': ComputePathThroughPoses.Goal().NO_VALID_PATH, - 'no_viapoints_given': ComputePathThroughPoses.Goal().NO_VIAPOINTS_GIVEN} + 'unknown': ComputePathThroughPoses.Result().UNKNOWN, + 'invalid_planner': ComputePathThroughPoses.Result().INVALID_PLANNER, + 'tf_error': ComputePathThroughPoses.Result().TF_ERROR, + 'start_outside_map': ComputePathThroughPoses.Result().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathThroughPoses.Result().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathThroughPoses.Result().START_OCCUPIED, + 'goal_occupied': ComputePathThroughPoses.Result().GOAL_OCCUPIED, + 'timeout': ComputePathThroughPoses.Result().TIMEOUT, + 'no_valid_path': ComputePathThroughPoses.Result().NO_VALID_PATH, + 'no_viapoints_given': ComputePathThroughPoses.Result().NO_VIAPOINTS_GIVEN} for planner, error_code in compute_path_through_poses.items(): result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) @@ -141,12 +141,12 @@ def main(argv=sys.argv[1:]): navigator._waitForNodeToActivate('smoother_server') smoother = { - 'invalid_smoother': SmoothPath.Goal().INVALID_SMOOTHER, - 'unknown': SmoothPath.Goal().UNKNOWN, - 'timeout': SmoothPath.Goal().TIMEOUT, - 'smoothed_path_in_collision': SmoothPath.Goal().SMOOTHED_PATH_IN_COLLISION, - 'failed_to_smooth_path': SmoothPath.Goal().FAILED_TO_SMOOTH_PATH, - 'invalid_path': SmoothPath.Goal().INVALID_PATH + 'invalid_smoother': SmoothPath.Result().INVALID_SMOOTHER, + 'unknown': SmoothPath.Result().UNKNOWN, + 'timeout': SmoothPath.Result().TIMEOUT, + 'smoothed_path_in_collision': SmoothPath.Result().SMOOTHED_PATH_IN_COLLISION, + 'failed_to_smooth_path': SmoothPath.Result().FAILED_TO_SMOOTH_PATH, + 'invalid_path': SmoothPath.Result().INVALID_PATH } for smoother, error_code in smoother.items(): diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 796047677e1..d2083fbb4f9 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -231,7 +231,7 @@ def main(argv=sys.argv[1:]): assert not result result = not result assert test.action_result.missed_waypoints[0].error_code == \ - ComputePathToPose.Goal().GOAL_OUTSIDE_MAP + ComputePathToPose.Result().GOAL_OUTSIDE_MAP # stop on failure test with bogous waypoint test.setStopFailureParam(True) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 2298aae03a8..fb214bd41c1 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -251,7 +251,8 @@ WaypointFollower::followWaypoints() nav2_msgs::msg::MissedWaypoint missedWaypoint; missedWaypoint.index = goal_index; missedWaypoint.goal = goal->poses[goal_index]; - missedWaypoint.error_code = nav2_msgs::action::FollowWaypoints::Goal::TASK_EXECUTOR_FAILED; + missedWaypoint.error_code = + nav2_msgs::action::FollowWaypoints::Result::TASK_EXECUTOR_FAILED; result->missed_waypoints.push_back(missedWaypoint); } // if task execution was failed and stop_on_failure_ is on , terminate action