From 94aaaf881b2063b95b0c3afe5ddd772b2ddca1e5 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 19 Oct 2022 19:41:15 -0400 Subject: [PATCH 01/61] issue with finding key --- .../bt_action_server_impl.hpp | 18 ++++++++++++++++++ nav2_msgs/action/NavigateThroughPoses.action | 1 + nav2_msgs/action/NavigateToPose.action | 1 + 3 files changed, 20 insertions(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index db619e1a701..a1ca6e864dd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -21,10 +21,12 @@ #include #include #include +#include #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "std_msgs/msg/int16.hpp" namespace nav2_behavior_tree { @@ -226,6 +228,22 @@ void BtActionServer::executeCallback() // Give server an opportunity to populate the result message or simple give // an indication that the action is complete. auto result = std::make_shared(); + + try + { + std::vector error_codes; + + // 0-99 error codes for follow path + error_codes.push_back(blackboard_->get("follow_path_error_code"); + + //100-199 error codes for compute path to pose + error_codes.push_back(blackboard_->get("compute_path_to_pose_error_code") + 100); + + result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); + } catch (std::exception & ex) { + RCLCPP_WARN(logger_, "Failed to get error_code: \"%s\"", ex.what()); + } + on_completion_callback_(result, rc); switch (rc) { diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index e2a57f25a94..33009f45a63 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -4,6 +4,7 @@ string behavior_tree --- #result definition std_msgs/Empty result +int16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index b7077922418..378eb8e7576 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -4,6 +4,7 @@ string behavior_tree --- #result definition std_msgs/Empty result +int16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose From 589460b5137889513c0d0fa1a32e736324cc99c9 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 20 Oct 2022 17:02:45 -0400 Subject: [PATCH 02/61] passed up codes to bt_navigator --- .../bt_action_server_impl.hpp | 18 +++++++----------- .../action/compute_path_to_pose_action.hpp | 2 -- .../plugins/action/follow_path_action.hpp | 2 -- .../action/compute_path_to_pose_action.cpp | 6 +++--- .../plugins/action/follow_path_action.cpp | 6 +++--- .../src/navigators/navigate_to_pose.cpp | 2 ++ 6 files changed, 15 insertions(+), 21 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index a1ca6e864dd..987e5c6b641 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -229,20 +229,16 @@ void BtActionServer::executeCallback() // an indication that the action is complete. auto result = std::make_shared(); - try - { - std::vector error_codes; + // Grab error codes from blackboard + std::vector error_codes; - // 0-99 error codes for follow path - error_codes.push_back(blackboard_->get("follow_path_error_code"); + // 0-99 error codes for follow path + error_codes.push_back(blackboard_->get("follow_path_error_code")); - //100-199 error codes for compute path to pose - error_codes.push_back(blackboard_->get("compute_path_to_pose_error_code") + 100); + //100-199 error codes for compute path to pose + error_codes.push_back(blackboard_->get("compute_path_to_pose_error_code") + 100); - result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); - } catch (std::exception & ex) { - RCLCPP_WARN(logger_, "Failed to get error_code: \"%s\"", ex.what()); - } + result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); on_completion_callback_(result, rc); 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 e980012cc48..fc95daaf439 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 @@ -75,8 +75,6 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), - BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), 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 369e7ab0f98..b103b835011 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 @@ -84,8 +84,6 @@ class FollowPathAction : public BtActionNode BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), - BT::OutputPort( - "follow_path_error_code", "The follow path error code"), }); } }; 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 b5e0e9084cb..335b982c2bd 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 @@ -42,7 +42,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() setOutput("path", result_.result->path); // Set empty error code, action was successful result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + config().blackboard->set("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } @@ -50,7 +50,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + config().blackboard->set("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -60,7 +60,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() setOutput("path", empty_path); // Set empty error code, action was cancelled result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + config().blackboard->set("compute_path_to_pose_error_code", result_.result->error_code); 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 c90d8fb65a1..8b1ccaa9d1e 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -37,20 +37,20 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { - setOutput("follow_path_error_code", ActionGoal::NONE); + config().blackboard->set("compute_path_to_pose_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } BT::NodeStatus FollowPathAction::on_aborted() { - setOutput("follow_path_error_code", result_.result->error_code); + config().blackboard->set("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } BT::NodeStatus FollowPathAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + config().blackboard->set("compute_path_to_pose_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 82727af5e2d..31c7d36ac8f 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -212,6 +212,8 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) start_time_ = clock_->now(); auto blackboard = bt_action_server_->getBlackboard(); blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set("follow_path_error_code", 0); + blackboard->set("compute_path_to_pose_error_code", 0); //NOLINT // Update the goal pose on the blackboard blackboard->set(goal_blackboard_id_, goal->pose); From a955deffa5609cb045b1a9d16f6aeb500cefd7e9 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 25 Oct 2022 20:01:49 -0400 Subject: [PATCH 03/61] lint fix --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 987e5c6b641..9fa888711c4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -235,7 +235,7 @@ void BtActionServer::executeCallback() // 0-99 error codes for follow path error_codes.push_back(blackboard_->get("follow_path_error_code")); - //100-199 error codes for compute path to pose + // 100-199 error codes for compute path to pose error_codes.push_back(blackboard_->get("compute_path_to_pose_error_code") + 100); result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); From 4b8293f3534c3dd3788092bce0f80033e4695956 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 26 Oct 2022 14:44:05 -0400 Subject: [PATCH 04/61] updates --- .../nav2_behavior_tree/bt_action_server.hpp | 3 ++ .../bt_action_server_impl.hpp | 33 +++++++++++++------ nav2_bt_navigator/src/bt_navigator.cpp | 2 -- .../src/navigators/navigate_to_pose.cpp | 1 - 4 files changed, 26 insertions(+), 13 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 75580803476..d781ace5cbe 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -189,6 +189,9 @@ class BtActionServer */ void executeCallback(); + + void populateErrorCode(typename std::shared_ptr result); + // Action name std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 9fa888711c4..15c011d32af 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -229,16 +229,7 @@ void BtActionServer::executeCallback() // an indication that the action is complete. auto result = std::make_shared(); - // Grab error codes from blackboard - std::vector error_codes; - - // 0-99 error codes for follow path - error_codes.push_back(blackboard_->get("follow_path_error_code")); - - // 100-199 error codes for compute path to pose - error_codes.push_back(blackboard_->get("compute_path_to_pose_error_code") + 100); - - result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); + populateErrorCode(result); on_completion_callback_(result, rc); @@ -260,6 +251,28 @@ void BtActionServer::executeCallback() } } +template +void BtActionServer::populateErrorCode(typename std::shared_ptr result) +{ + // Grab error codes from blackboard + std::vector error_codes; + + // 0-99 error codes for follow path + error_codes.push_back(blackboard_->get("follow_path_error_code")); + + // 100-199 error codes for compute path to pose or compute path through poses + int compute_path_error_code; + if (blackboard_->get("compute_path_to_pose_error_code", compute_path_error_code)){ + error_codes.push_back(compute_path_error_code + 100); + } else + { + error_codes.push_back(blackboard_->get("compute_path_through_pose_error_code") + 100); + } + + result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); +} + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 7c83ff9d9a2..3c6286ba013 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -17,8 +17,6 @@ #include #include #include -#include -#include #include #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 31c7d36ac8f..263f436896b 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" From cbdac4c9ac784fd7a5ea09368ef049cb4095eb81 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 15:15:14 -0400 Subject: [PATCH 05/61] adding error_code_id back in --- .../nav2_behavior_tree/bt_action_server_impl.hpp | 16 ++++++++-------- .../action/compute_path_through_poses_action.hpp | 2 +- .../action/compute_path_to_pose_action.hpp | 3 +-- .../plugins/action/follow_path_action.hpp | 2 ++ .../action/compute_path_through_poses_action.cpp | 6 +++--- .../action/compute_path_to_pose_action.cpp | 6 +++--- .../plugins/action/follow_path_action.cpp | 6 +++--- ...avigate_to_pose_w_replanning_and_recovery.xml | 4 ++-- .../src/navigators/navigate_to_pose.cpp | 2 -- 9 files changed, 23 insertions(+), 24 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 15c011d32af..aa8d601c698 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -261,14 +261,14 @@ void BtActionServer::populateErrorCode(typename std::shared_ptrget("follow_path_error_code")); - // 100-199 error codes for compute path to pose or compute path through poses - int compute_path_error_code; - if (blackboard_->get("compute_path_to_pose_error_code", compute_path_error_code)){ - error_codes.push_back(compute_path_error_code + 100); - } else - { - error_codes.push_back(blackboard_->get("compute_path_through_pose_error_code") + 100); - } +// // 100-199 error codes for compute path to pose or compute path through poses +// int compute_path_error_code; +// if (blackboard_->get("compute_path_to_pose_error_code", compute_path_error_code)){ +// error_codes.push_back(compute_path_error_code + 100); +// } else +// { +// error_codes.push_back(blackboard_->get("compute_path_through_pose_error_code") + 100); +// } result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); } 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 ad5954b6a4f..0b5f3b0972e 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 @@ -86,7 +86,7 @@ class ComputePathThroughPosesAction "Mapped name to the planner plugin type to use"), BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::OutputPort( - "compute_path_through_poses_error_code", "The compute path through poses error code"), + "error_code_id", "The compute path through poses error code"), }); } }; 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 5779871622c..829fd044282 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 @@ -78,7 +78,6 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), @@ -87,7 +86,7 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), + "error_code_id", "The compute path to pose error code"), }); } }; 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 b103b835011..b5f6ef4b972 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 @@ -84,6 +84,8 @@ class FollowPathAction : public BtActionNode BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), + BT::OutputPort( + "error_code_id", "The follow path error code"), }); } }; 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 ffcbd328aea..5fede84f2cf 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("compute_path_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -50,7 +50,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("compute_path_through_poses_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -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("compute_path_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::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 dca3718373e..0619a41ee06 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("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -49,7 +49,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - config().blackboard->set("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -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("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::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 8b1ccaa9d1e..3649fad8a1b 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -37,20 +37,20 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { - config().blackboard->set("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } BT::NodeStatus FollowPathAction::on_aborted() { - config().blackboard->set("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } BT::NodeStatus FollowPathAction::on_cancelled() { // Set empty error code, action was cancelled - config().blackboard->set("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 2396b844aed..d50bd7fe5df 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -10,12 +10,12 @@ - + - + diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 263f436896b..226a6735123 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -211,8 +211,6 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) start_time_ = clock_->now(); auto blackboard = bt_action_server_->getBlackboard(); blackboard->set("number_recoveries", 0); // NOLINT - blackboard->set("follow_path_error_code", 0); - blackboard->set("compute_path_to_pose_error_code", 0); //NOLINT // Update the goal pose on the blackboard blackboard->set(goal_blackboard_id_, goal->pose); From 7cd843a4c2bf787a2f3e5b2299041d7768e9ed33 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 16:30:13 -0400 Subject: [PATCH 06/61] error codes names in params --- .../nav2_behavior_tree/bt_action_server.hpp | 3 ++ .../bt_action_server_impl.hpp | 38 +++++++++++-------- nav2_bringup/params/nav2_params.yaml | 3 ++ ...gate_to_pose_w_replanning_and_recovery.xml | 4 +- 4 files changed, 30 insertions(+), 18 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index d781ace5cbe..73982f236b8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -214,6 +214,9 @@ class BtActionServer // Libraries to pull plugins (BT Nodes) from std::vector plugin_lib_names_; + // Error code id names + std::vector error_code_id_names_; + // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index aa8d601c698..9ec95e05655 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -61,6 +61,15 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + + std::vector error_code_id_names = { + "follow_path_error_code_id", + "compute_path_error_code_id" + }; + + if (!node->has_parameter("error_code_id_names")) { + node->declare_parameter("error_code_id_names", error_code_id_names); + } } template @@ -106,6 +115,8 @@ bool BtActionServer::on_configure() node->get_parameter("default_server_timeout", timeout); default_server_timeout_ = std::chrono::milliseconds(timeout); + error_code_id_names_ = node->get_parameter("error_code_id_names").as_string_array(); + // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -255,22 +266,17 @@ template void BtActionServer::populateErrorCode(typename std::shared_ptr result) { - // Grab error codes from blackboard - std::vector error_codes; - - // 0-99 error codes for follow path - error_codes.push_back(blackboard_->get("follow_path_error_code")); - -// // 100-199 error codes for compute path to pose or compute path through poses -// int compute_path_error_code; -// if (blackboard_->get("compute_path_to_pose_error_code", compute_path_error_code)){ -// error_codes.push_back(compute_path_error_code + 100); -// } else -// { -// error_codes.push_back(blackboard_->get("compute_path_through_pose_error_code") + 100); -// } - - result->error_code = std::accumulate(error_codes.begin(), error_codes.end(), 0); + int highest_prority_error_code = 0; + for(auto error_code_id_name : error_code_id_names_) + { + int current_error_code = blackboard_->get(error_code_id_name); + if (current_error_code > highest_prority_error_code) + { + highest_prority_error_code = current_error_code; + } + } + + result->error_code = highest_prority_error_code; } } // namespace nav2_behavior_tree diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 0bd4f4e45ba..f90ceb6d734 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -96,6 +96,9 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node + error_code_id_names: + - follow_path_error_code_id + - compute_path_error_code_id controller_server: ros__parameters: diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index d50bd7fe5df..412a4b7bfa6 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -10,12 +10,12 @@ - + - + From d37581ff7ca487b7f92ee4f0dae8077ff04da72a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 16:43:14 -0400 Subject: [PATCH 07/61] bump error codes --- .../action/ComputePathThroughPoses.action | 20 +++++++++---------- nav2_msgs/action/ComputePathToPose.action | 18 ++++++++--------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index c37297c69e7..7d9278d06a0 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,15 +1,15 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 START_OUTSIDE_MAP=3 -int16 GOAL_OUTSIDE_MAP=4 -int16 START_OCCUPIED=5 -int16 GOAL_OCCUPIED=6 -int16 TIMEOUT=7 -int16 NO_VALID_PATH=8 -int16 NO_VIAPOINTS_GIVEN=9 +int16 NONE=2000 +int16 UNKNOWN=2001 +int16 TF_ERROR=2002 +int16 START_OUTSIDE_MAP=2003 +int16 GOAL_OUTSIDE_MAP=2004 +int16 START_OCCUPIED=2005 +int16 GOAL_OCCUPIED=2006 +int16 TIMEOUT=2007 +int16 NO_VALID_PATH=2008 +int16 NO_VIAPOINTS_GIVEN=2009 #goal definition geometry_msgs/PoseStamped[] goals diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 7e727088a15..157c92f99cb 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,14 +1,14 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 START_OUTSIDE_MAP=3 -int16 GOAL_OUTSIDE_MAP=4 -int16 START_OCCUPIED=5 -int16 GOAL_OCCUPIED=6 -int16 TIMEOUT=7 -int16 NO_VALID_PATH=8 +int16 NONE=1000 +int16 UNKNOWN=1001 +int16 TF_ERROR=1002 +int16 START_OUTSIDE_MAP=1003 +int16 GOAL_OUTSIDE_MAP=1004 +int16 START_OCCUPIED=1005 +int16 GOAL_OCCUPIED=1006 +int16 TIMEOUT=1007 +int16 NO_VALID_PATH=1008 #goal definition geometry_msgs/PoseStamped goal From def6e6d9983c6e3cefae05d986a95253ca665801 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 18:39:02 -0400 Subject: [PATCH 08/61] lint --- .../bt_action_server_impl.hpp | 25 ++++++++++++------- .../plugins/action/follow_path_action.hpp | 2 +- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 9ec95e05655..58eaf2cd3d0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -63,8 +63,8 @@ BtActionServer::BtActionServer( } std::vector error_code_id_names = { - "follow_path_error_code_id", - "compute_path_error_code_id" + "follow_path_error_code_id", + "compute_path_error_code_id" }; if (!node->has_parameter("error_code_id_names")) { @@ -263,15 +263,22 @@ void BtActionServer::executeCallback() } template -void BtActionServer::populateErrorCode(typename std::shared_ptr result) +void BtActionServer::populateErrorCode( + typename std::shared_ptr result) { int highest_prority_error_code = 0; - for(auto error_code_id_name : error_code_id_names_) - { - int current_error_code = blackboard_->get(error_code_id_name); - if (current_error_code > highest_prority_error_code) - { + for (auto error_code_id_name : error_code_id_names_) { + int current_error_code = 0; + try { + current_error_code = blackboard_->get(error_code_id_name); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR( + logger_, + "Failed to get %s from blackboard: \"%s\"", + error_code_id_name.c_str(), + ex.what()); + } + if (current_error_code > highest_prority_error_code) { highest_prority_error_code = current_error_code; } } 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 b5f6ef4b972..4460b0e9a41 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 @@ -85,7 +85,7 @@ class FollowPathAction : public BtActionNode BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), BT::OutputPort( - "error_code_id", "The follow path error code"), + "error_code_id", "The follow path error code"), }); } }; From bb258cacc4d2668175de2b3b0351144b1e5e812c Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 18:51:51 -0400 Subject: [PATCH 09/61] spelling --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 58eaf2cd3d0..46539517d57 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -266,7 +266,7 @@ template void BtActionServer::populateErrorCode( typename std::shared_ptr result) { - int highest_prority_error_code = 0; + int highest_priority_error_code = 0; for (auto error_code_id_name : error_code_id_names_) { int current_error_code = 0; try { @@ -278,12 +278,12 @@ void BtActionServer::populateErrorCode( error_code_id_name.c_str(), ex.what()); } - if (current_error_code > highest_prority_error_code) { - highest_prority_error_code = current_error_code; + if (current_error_code > highest_priority_error_code) { + highest_priority_error_code = current_error_code; } } - result->error_code = highest_prority_error_code; + result->error_code = highest_priority_error_code; } } // namespace nav2_behavior_tree From 281f5ce136ca2acb7e7772dc811cbe8a8d11904e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 19:54:33 -0400 Subject: [PATCH 10/61] test fix --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 46539517d57..6cccdf7e20f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -271,12 +271,11 @@ void BtActionServer::populateErrorCode( int current_error_code = 0; try { current_error_code = blackboard_->get(error_code_id_name); - } catch (const std::runtime_error & ex) { + } catch (...) { RCLCPP_ERROR( logger_, - "Failed to get %s from blackboard: \"%s\"", - error_code_id_name.c_str(), - ex.what()); + "Failed to get %s from blackboard", + error_code_id_name.c_str()); } if (current_error_code > highest_priority_error_code) { highest_priority_error_code = current_error_code; From 1af29e631dc430e7beea2bf01aa7d7cc072e6988 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 7 Nov 2022 08:13:53 -0500 Subject: [PATCH 11/61] update behavior trees --- nav2_bt_navigator/behavior_trees/follow_point.xml | 4 ++-- ..._consistent_replanning_and_if_path_becomes_invalid.xml | 4 ++-- .../navigate_through_poses_w_replanning_and_recovery.xml | 4 ++-- ...te_to_pose_w_replanning_goal_patience_and_recovery.xml | 8 ++++---- ...covery_and_replanning_only_if_path_becomes_invalid.xml | 4 ++-- .../behavior_trees/navigate_w_replanning_distance.xml | 4 ++-- .../navigate_w_replanning_only_if_goal_is_updated.xml | 4 ++-- ...navigate_w_replanning_only_if_path_becomes_invalid.xml | 6 +++--- .../behavior_trees/navigate_w_replanning_speed.xml | 4 ++-- .../behavior_trees/navigate_w_replanning_time.xml | 4 ++-- 10 files changed, 23 insertions(+), 23 deletions(-) diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ffae8d9b2ab..c61d59a5199 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -8,13 +8,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index f4ed518444d..fb79bdd70c4 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -19,13 +19,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 1378863cdf6..9b881e70bb4 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -11,13 +11,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 6b5883bfe70..8af39bbe2be 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -1,8 +1,8 @@ @@ -11,7 +11,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index d21efa3b4bf..b16809e7e53 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -17,13 +17,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index 8ca5dbbc5ac..b9db08c8c48 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 3e889440a88..1fd2b65fd10 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index e921db55578..c9f1cce9035 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -5,17 +5,17 @@ - + - + - + \ No newline at end of file diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 049e78794b8..b85d39b682c 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 33c2c411ad0..6df23df699d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -6,9 +6,9 @@ - + - + From 2ae9b9516ebfc7c732182ef21b003586e6a92518 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 7 Nov 2022 08:17:57 -0500 Subject: [PATCH 12/61] cleanup --- .../include/nav2_behavior_tree/bt_action_server.hpp | 6 +++++- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 5 ++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 73982f236b8..61db11192b4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -189,7 +189,11 @@ class BtActionServer */ void executeCallback(); - + /** + * \brief updates the action server result to the highest priority error code posted on the + * blackboard + * \param result the action server result to be updated + */ void populateErrorCode(typename std::shared_ptr result); // Action name diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 6cccdf7e20f..0d9f866c3a8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -21,12 +21,10 @@ #include #include #include -#include #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" -#include "std_msgs/msg/int16.hpp" namespace nav2_behavior_tree { @@ -115,6 +113,7 @@ bool BtActionServer::on_configure() node->get_parameter("default_server_timeout", timeout); default_server_timeout_ = std::chrono::milliseconds(timeout); + // Get error code id names to grab off of the blackboard error_code_id_names_ = node->get_parameter("error_code_id_names").as_string_array(); // Create the class that registers our custom nodes and executes the BT @@ -267,7 +266,7 @@ void BtActionServer::populateErrorCode( typename std::shared_ptr result) { int highest_priority_error_code = 0; - for (auto error_code_id_name : error_code_id_names_) { + for (const auto &error_code_id_name : error_code_id_names_) { int current_error_code = 0; try { current_error_code = blackboard_->get(error_code_id_name); From bbc9c866cb8ec265ce60390a7761b73d32b9750b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 7 Nov 2022 13:34:03 -0500 Subject: [PATCH 13/61] Update bt_action_server_impl.hpp --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 0d9f866c3a8..d94e742d6d9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -266,7 +266,7 @@ void BtActionServer::populateErrorCode( typename std::shared_ptr result) { int highest_priority_error_code = 0; - for (const auto &error_code_id_name : error_code_id_names_) { + for (const auto & error_code_id_name : error_code_id_names_) { int current_error_code = 0; try { current_error_code = blackboard_->get(error_code_id_name); From be0512431d842bec0608a7ff1822b01084e6ff12 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 7 Nov 2022 19:14:41 -0500 Subject: [PATCH 14/61] code review --- .../nav2_behavior_tree/bt_action_server.hpp | 2 +- .../bt_action_server_impl.hpp | 28 ++++-- nav2_bringup/params/nav2_params.yaml | 96 +++++++++---------- .../action/ComputePathThroughPoses.action | 20 ++-- nav2_msgs/action/ComputePathToPose.action | 18 ++-- nav2_msgs/action/FollowPath.action | 12 +-- nav2_msgs/action/FollowWaypoints.action | 1 + .../waypoint_follower.hpp | 2 + .../src/waypoint_follower.cpp | 3 + 9 files changed, 100 insertions(+), 82 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 61db11192b4..166ce53e0e0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -219,7 +219,7 @@ class BtActionServer std::vector plugin_lib_names_; // Error code id names - std::vector error_code_id_names_; + std::vector error_code_ids_; // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index d94e742d6d9..869f0344070 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" @@ -66,6 +67,12 @@ BtActionServer::BtActionServer( }; if (!node->has_parameter("error_code_id_names")) { + RCLCPP_INFO(logger_, "Error_code_ids parameter is not set. Using default values of: "); + for(auto const & error_code_id : error_code_id_names) { + RCLCPP_INFO(logger_, error_code_id.c_str()); + } + RCLCPP_INFO(logger_, "Make sure these match your BT and there are not other sources of error" + " codes you want reported to your application"); node->declare_parameter("error_code_id_names", error_code_id_names); } } @@ -114,7 +121,7 @@ bool BtActionServer::on_configure() default_server_timeout_ = std::chrono::milliseconds(timeout); // Get error code id names to grab off of the blackboard - error_code_id_names_ = node->get_parameter("error_code_id_names").as_string_array(); + error_code_ids_ = node->get_parameter("error_code_id_names").as_string_array(); // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -265,23 +272,28 @@ template void BtActionServer::populateErrorCode( typename std::shared_ptr result) { - int highest_priority_error_code = 0; - for (const auto & error_code_id_name : error_code_id_names_) { - int current_error_code = 0; + int highest_priority_error_code = std::numeric_limits::max(); + for (const auto & error_code_id_name : error_code_ids_) { + int current_error_code = std::numeric_limits::max(); try { current_error_code = blackboard_->get(error_code_id_name); + + if (current_error_code != 0 && current_error_code < highest_priority_error_code) { + highest_priority_error_code = current_error_code; + } } catch (...) { RCLCPP_ERROR( logger_, "Failed to get %s from blackboard", error_code_id_name.c_str()); } - if (current_error_code > highest_priority_error_code) { - highest_priority_error_code = current_error_code; - } } - result->error_code = highest_priority_error_code; + if (highest_priority_error_code != std::numeric_limits::max()) { + result->error_code = highest_priority_error_code; + } else { + result->error_code = 0; + } } } // namespace nav2_behavior_tree diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index f90ceb6d734..da342c43cab 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -50,55 +50,55 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node error_code_id_names: - - follow_path_error_code_id - - compute_path_error_code_id + - follow_path_error_code_id + - compute_path_error_code_id controller_server: ros__parameters: diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 7d9278d06a0..4b682dae9b6 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,15 +1,15 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=2000 -int16 UNKNOWN=2001 -int16 TF_ERROR=2002 -int16 START_OUTSIDE_MAP=2003 -int16 GOAL_OUTSIDE_MAP=2004 -int16 START_OCCUPIED=2005 -int16 GOAL_OCCUPIED=2006 -int16 TIMEOUT=2007 -int16 NO_VALID_PATH=2008 -int16 NO_VIAPOINTS_GIVEN=2009 +int16 NONE=0 +int16 UNKNOWN=3000 +int16 TF_ERROR=3001 +int16 START_OUTSIDE_MAP=3002 +int16 GOAL_OUTSIDE_MAP=3003 +int16 START_OCCUPIED=3004 +int16 GOAL_OCCUPIED=3005 +int16 TIMEOUT=3006 +int16 NO_VALID_PATH=3007 +int16 NO_VIAPOINTS_GIVEN=3008 #goal definition geometry_msgs/PoseStamped[] goals diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 157c92f99cb..57dca475b1f 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,14 +1,14 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=1000 -int16 UNKNOWN=1001 -int16 TF_ERROR=1002 -int16 START_OUTSIDE_MAP=1003 -int16 GOAL_OUTSIDE_MAP=1004 -int16 START_OCCUPIED=1005 -int16 GOAL_OCCUPIED=1006 -int16 TIMEOUT=1007 -int16 NO_VALID_PATH=1008 +int16 NONE=0 +int16 UNKNOWN=2000 +int16 TF_ERROR=2001 +int16 START_OUTSIDE_MAP=2002 +int16 GOAL_OUTSIDE_MAP=2003 +int16 START_OCCUPIED=2004 +int16 GOAL_OCCUPIED=2005 +int16 TIMEOUT=2006 +int16 NO_VALID_PATH=2007 #goal definition geometry_msgs/PoseStamped goal diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 9ab82ff4d76..f345a4e0d2e 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,12 +1,12 @@ # Error codes # Note: The expected priority order of the errors should match the message order int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 INVALID_PATH=3 -int16 PATIENCE_EXCEEDED=4 -int16 FAILED_TO_MAKE_PROGRESS=5 -int16 NO_VALID_CONTROL=6 +int16 UNKNOWN=1000 +int16 TF_ERROR=1001 +int16 INVALID_PATH=1002 +int16 PATIENCE_EXCEEDED=1003 +int16 FAILED_TO_MAKE_PROGRESS=1004 +int16 NO_VALID_CONTROL=1005 #goal definition nav_msgs/Path path diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index c4b47648219..dc600c7ddbd 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -3,6 +3,7 @@ geometry_msgs/PoseStamped[] poses --- #result definition int32[] missed_waypoints +int16 error_code --- #feedback definition uint32 current_waypoint diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index b7cd82d684a..2ff18709d15 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -133,6 +133,8 @@ class WaypointFollower : public nav2_util::LifecycleNode rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; + int error_code_; + bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a04c404c772..2351ed3e261 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -220,6 +220,7 @@ WaypointFollower::followWaypoints() "list and stop on failure is enabled." " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; + result->error_code = error_code_; action_server_->terminate_current(result); failed_ids_.clear(); return; @@ -245,6 +246,7 @@ WaypointFollower::followWaypoints() " stop on failure is enabled." " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; + result->error_code = error_code_; action_server_->terminate_current(result); failed_ids_.clear(); return; @@ -303,6 +305,7 @@ WaypointFollower::resultCallback( return; case rclcpp_action::ResultCode::CANCELED: current_goal_status_ = ActionStatus::FAILED; + error_code_ = result.result->error_code; return; default: current_goal_status_ = ActionStatus::UNKNOWN; From 5298bcfb72fdda147bc698cbfd991e783f78bb9e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 8 Nov 2022 08:04:55 -0500 Subject: [PATCH 15/61] lint --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 869f0344070..3952f598e72 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" @@ -68,11 +68,12 @@ BtActionServer::BtActionServer( if (!node->has_parameter("error_code_id_names")) { RCLCPP_INFO(logger_, "Error_code_ids parameter is not set. Using default values of: "); - for(auto const & error_code_id : error_code_id_names) { + for (auto const & error_code_id : error_code_id_names) { RCLCPP_INFO(logger_, error_code_id.c_str()); } - RCLCPP_INFO(logger_, "Make sure these match your BT and there are not other sources of error" - " codes you want reported to your application"); + RCLCPP_INFO( + logger_, "Make sure these match your BT and there are not other sources of error" + " codes you want reported to your application"); node->declare_parameter("error_code_id_names", error_code_id_names); } } From 9a7c87681c536c3a079882b9d296308d202c7725 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 8 Nov 2022 08:15:05 -0500 Subject: [PATCH 16/61] code review --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 3952f598e72..db8c78616a6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -292,8 +292,6 @@ void BtActionServer::populateErrorCode( if (highest_priority_error_code != std::numeric_limits::max()) { result->error_code = highest_priority_error_code; - } else { - result->error_code = 0; } } From e21a6bedcc7f51c9c84c25807c899fe80a0ed1b4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 8 Nov 2022 13:11:48 -0500 Subject: [PATCH 17/61] log fix --- .../nav2_behavior_tree/bt_action_server_impl.hpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index db8c78616a6..b62468cbb33 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -26,6 +26,7 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +//#include "nav2_util/print_utils.hpp" namespace nav2_behavior_tree { @@ -67,13 +68,15 @@ BtActionServer::BtActionServer( }; if (!node->has_parameter("error_code_id_names")) { - RCLCPP_INFO(logger_, "Error_code_ids parameter is not set. Using default values of: "); - for (auto const & error_code_id : error_code_id_names) { - RCLCPP_INFO(logger_, error_code_id.c_str()); + std::string error_code_ids_str; + for(const auto & error_code_id : error_code_id_names) { + error_code_ids_str += error_code_id + "\n"; } - RCLCPP_INFO( - logger_, "Make sure these match your BT and there are not other sources of error" - " codes you want reported to your application"); + RCLCPP_INFO_STREAM( + logger_, "Error_code_ids parameter is not set. Using default values of: " + << error_code_ids_str + << "Make sure these match your BT and there are not other sources of error codes you want " + "reported to your application"); node->declare_parameter("error_code_id_names", error_code_id_names); } } From 9beff9aa6bdf0d4cadd19747180351ca315c623a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 8 Nov 2022 15:04:55 -0500 Subject: [PATCH 18/61] error code for waypoint follower --- nav2_system_tests/src/waypoint_follower/tester.py | 11 +++++++---- nav2_waypoint_follower/src/waypoint_follower.cpp | 15 +++++++++++++-- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index e6d299caa55..a7419d4c77b 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -19,6 +19,7 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from nav2_msgs.action import FollowWaypoints +from nav2_msgs.action import ComputePathToPose from nav2_msgs.srv import ManageLifecycleNodes import rclpy @@ -103,16 +104,17 @@ def run(self, block): try: rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status - result = get_result_future.result().result + self.result = get_result_future.result().result + self.info_msg('Error code: {0}' .format(self.result.error_code)) except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') if status != GoalStatus.STATUS_SUCCEEDED: self.info_msg(f'Goal failed with status code: {status}') return False - if len(result.missed_waypoints) > 0: + if len(self.result.missed_waypoints) > 0: self.info_msg('Goal failed to process all waypoints,' - ' missed {0} wps.'.format(len(result.missed_waypoints))) + ' missed {0} wps.'.format(len(self.result.missed_waypoints))) return False self.info_msg('Goal succeeded!') @@ -208,12 +210,13 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() - # a failure case + # Set a waypoint outside the bounds of the map time.sleep(2) test.setWaypoints([[100.0, 100.0]]) result = test.run(True) assert not result result = not result + assert test.result.error_code == ComputePathToPose.Goal().GOAL_OUTSIDE_MAP test.shutdown() test.info_msg('Done Shutting Down.') diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 2351ed3e261..64059e91578 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -221,8 +221,10 @@ WaypointFollower::followWaypoints() " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; result->error_code = error_code_; + RCLCPP_INFO_STREAM(get_logger(), "Way Error code" << error_code_); action_server_->terminate_current(result); failed_ids_.clear(); + error_code_ = 0; return; } else { RCLCPP_INFO( @@ -247,7 +249,9 @@ WaypointFollower::followWaypoints() " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; result->error_code = error_code_; + RCLCPP_INFO_STREAM(get_logger(), "Way Error code" << error_code_); action_server_->terminate_current(result); + error_code_ = 0; failed_ids_.clear(); return; } else { @@ -268,8 +272,14 @@ WaypointFollower::followWaypoints() get_logger(), "Completed all %zu waypoints requested.", goal->poses.size()); result->missed_waypoints = failed_ids_; - action_server_->succeeded_current(result); + if (error_code_ > 0) { + result->error_code = error_code_; + action_server_->terminate_current(result); + } else { + action_server_->succeeded_current(result); + } failed_ids_.clear(); + error_code_ = 0; return; } } else { @@ -301,11 +311,12 @@ WaypointFollower::resultCallback( current_goal_status_ = ActionStatus::SUCCEEDED; return; case rclcpp_action::ResultCode::ABORTED: + RCLCPP_INFO(get_logger(), "ABORTED WAYPOINT RESULT"); current_goal_status_ = ActionStatus::FAILED; + error_code_ = result.result->error_code; return; case rclcpp_action::ResultCode::CANCELED: current_goal_status_ = ActionStatus::FAILED; - error_code_ = result.result->error_code; return; default: current_goal_status_ = ActionStatus::UNKNOWN; From fcffaf85f79e752156359667ceaf58d185213c4f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 8 Nov 2022 15:06:16 -0500 Subject: [PATCH 19/61] clean up --- nav2_system_tests/src/waypoint_follower/tester.py | 1 - nav2_waypoint_follower/src/waypoint_follower.cpp | 3 --- 2 files changed, 4 deletions(-) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index a7419d4c77b..da2475ced52 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -105,7 +105,6 @@ def run(self, block): rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status self.result = get_result_future.result().result - self.info_msg('Error code: {0}' .format(self.result.error_code)) except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 64059e91578..56908deaabf 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -221,7 +221,6 @@ WaypointFollower::followWaypoints() " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; result->error_code = error_code_; - RCLCPP_INFO_STREAM(get_logger(), "Way Error code" << error_code_); action_server_->terminate_current(result); failed_ids_.clear(); error_code_ = 0; @@ -249,7 +248,6 @@ WaypointFollower::followWaypoints() " Terminating action.", goal_index); result->missed_waypoints = failed_ids_; result->error_code = error_code_; - RCLCPP_INFO_STREAM(get_logger(), "Way Error code" << error_code_); action_server_->terminate_current(result); error_code_ = 0; failed_ids_.clear(); @@ -311,7 +309,6 @@ WaypointFollower::resultCallback( current_goal_status_ = ActionStatus::SUCCEEDED; return; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_INFO(get_logger(), "ABORTED WAYPOINT RESULT"); current_goal_status_ = ActionStatus::FAILED; error_code_ = result.result->error_code; return; From 850ac8ee9f04361ff5df99bf14a0ed59e9671641 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 9 Nov 2022 08:54:14 -0500 Subject: [PATCH 20/61] remove waypoint error test, too flaky on CI --- nav2_system_tests/src/waypoint_follower/tester.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index da2475ced52..0be137f4210 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -215,8 +215,7 @@ def main(argv=sys.argv[1:]): result = test.run(True) assert not result result = not result - assert test.result.error_code == ComputePathToPose.Goal().GOAL_OUTSIDE_MAP - + test.shutdown() test.info_msg('Done Shutting Down.') From 69e3d5eaefcf9298181b5d849a6e49a40fb1b921 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 11 Nov 2022 10:52:07 -0500 Subject: [PATCH 21/61] lint and code review --- .../nav2_behavior_tree/bt_action_server_impl.hpp | 16 ++++++++-------- .../src/waypoint_follower/tester.py | 3 +-- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index b62468cbb33..691d144e596 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -26,7 +26,6 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" -//#include "nav2_util/print_utils.hpp" namespace nav2_behavior_tree { @@ -278,18 +277,19 @@ void BtActionServer::populateErrorCode( { int highest_priority_error_code = std::numeric_limits::max(); for (const auto & error_code_id_name : error_code_ids_) { - int current_error_code = std::numeric_limits::max(); - try { - current_error_code = blackboard_->get(error_code_id_name); + int current_error_code; + bool found_error_code = blackboard_->get(error_code_id_name, current_error_code); + if (found_error_code) { if (current_error_code != 0 && current_error_code < highest_priority_error_code) { highest_priority_error_code = current_error_code; } - } catch (...) { + } + else { RCLCPP_ERROR( - logger_, - "Failed to get %s from blackboard", - error_code_id_name.c_str()); + logger_, + "Failed to get error code: %s from blackboard", + error_code_id_name.c_str()); } } diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 0be137f4210..4f0ce014c4f 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -19,7 +19,6 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from nav2_msgs.action import FollowWaypoints -from nav2_msgs.action import ComputePathToPose from nav2_msgs.srv import ManageLifecycleNodes import rclpy @@ -215,7 +214,7 @@ def main(argv=sys.argv[1:]): result = test.run(True) assert not result result = not result - + test.shutdown() test.info_msg('Done Shutting Down.') From 2d7fd055cf4326215bfd25e541a21b4109c8674e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 11 Nov 2022 13:04:51 -0500 Subject: [PATCH 22/61] rough imp for waypoint changes --- nav2_msgs/action/FollowWaypoints.action | 2 +- .../waypoint_follower.hpp | 9 ++++- .../src/waypoint_follower.cpp | 40 ++++++++++++------- 3 files changed, 34 insertions(+), 17 deletions(-) diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index dc600c7ddbd..daab47b4a9e 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -3,7 +3,7 @@ geometry_msgs/PoseStamped[] poses --- #result definition int32[] missed_waypoints -int16 error_code +int16[] error_code --- #feedback definition uint32 current_waypoint diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 2ff18709d15..14d406717a9 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -43,6 +43,12 @@ enum class ActionStatus SUCCEEDED = 3 }; +struct MissedWaypoint +{ + unsigned int index; + int error_code; +}; + /** * @class nav2_waypoint_follower::WaypointFollower * @brief An action server that uses behavior tree for navigating a robot to its @@ -138,7 +144,8 @@ class WaypointFollower : public nav2_util::LifecycleNode bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; - std::vector failed_ids_; +// std::vector failed_ids_; + std::vector missed_waypoints_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 56908deaabf..7739a17be3b 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -212,17 +212,21 @@ WaypointFollower::followWaypoints() action_server_->publish_feedback(feedback); if (current_goal_status_ == ActionStatus::FAILED) { - failed_ids_.push_back(goal_index); + missed_waypoints_.emplace_back(MissedWaypoint{goal_index, error_code_}); if (stop_on_failure_) { RCLCPP_WARN( get_logger(), "Failed to process waypoint %i in waypoint " "list and stop on failure is enabled." " Terminating action.", goal_index); - result->missed_waypoints = failed_ids_; - result->error_code = error_code_; + + for (const auto missed_waypoint : missed_waypoints_) { + result->missed_waypoints.push_back(missed_waypoint.index); + result->error_code.push_back(missed_waypoint.error_code); + } + action_server_->terminate_current(result); - failed_ids_.clear(); + missed_waypoints_.clear(); error_code_ = 0; return; } else { @@ -239,18 +243,24 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); + if (!is_task_executed) { + missed_waypoints_.emplace_back(MissedWaypoint{goal_index, error_code_}); + } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { - failed_ids_.push_back(goal_index); RCLCPP_WARN( get_logger(), "Failed to execute task at waypoint %i " " stop on failure is enabled." " Terminating action.", goal_index); - result->missed_waypoints = failed_ids_; - result->error_code = error_code_; + + for (const auto missed_waypoint : missed_waypoints_) { + result->missed_waypoints.push_back(missed_waypoint.index); + result->error_code.push_back(missed_waypoint.error_code); + } + action_server_->terminate_current(result); + missed_waypoints_.clear(); error_code_ = 0; - failed_ids_.clear(); return; } else { RCLCPP_INFO( @@ -269,14 +279,14 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", goal->poses.size()); - result->missed_waypoints = failed_ids_; - if (error_code_ > 0) { - result->error_code = error_code_; - action_server_->terminate_current(result); - } else { - action_server_->succeeded_current(result); + + for (const auto missed_waypoint : missed_waypoints_) { + result->missed_waypoints.push_back(missed_waypoint.index); + result->error_code.push_back(missed_waypoint.error_code); } - failed_ids_.clear(); + + action_server_->succeeded_current(result); + missed_waypoints_.clear(); error_code_ = 0; return; } From 02590bb340983950f6f1f1ae5f4b278cabc99091 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 11 Nov 2022 13:07:07 -0500 Subject: [PATCH 23/61] lint --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 691d144e596..0413313c6b1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -284,8 +284,7 @@ void BtActionServer::populateErrorCode( if (current_error_code != 0 && current_error_code < highest_priority_error_code) { highest_priority_error_code = current_error_code; } - } - else { + } else { RCLCPP_ERROR( logger_, "Failed to get error code: %s from blackboard", From f2c39f701cbc65f895e9689a04fe0c0b45c963d0 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 11 Nov 2022 16:50:44 -0500 Subject: [PATCH 24/61] code review --- .../bt_action_server_impl.hpp | 17 ++++---- nav2_msgs/action/FollowWaypoints.action | 2 +- .../waypoint_follower.hpp | 12 ++++-- .../src/waypoint_follower.cpp | 39 +++++++++---------- 4 files changed, 35 insertions(+), 35 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 0413313c6b1..27ce5ac76a3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -68,14 +68,14 @@ BtActionServer::BtActionServer( if (!node->has_parameter("error_code_id_names")) { std::string error_code_ids_str; - for(const auto & error_code_id : error_code_id_names) { + for (const auto & error_code_id : error_code_id_names) { error_code_ids_str += error_code_id + "\n"; } RCLCPP_INFO_STREAM( logger_, "Error_code_ids parameter is not set. Using default values of: " - << error_code_ids_str - << "Make sure these match your BT and there are not other sources of error codes you want " - "reported to your application"); + << error_code_ids_str + << "Make sure these match your BT and there are not other sources of error codes you want " + "reported to your application"); node->declare_parameter("error_code_id_names", error_code_id_names); } } @@ -278,17 +278,16 @@ void BtActionServer::populateErrorCode( int highest_priority_error_code = std::numeric_limits::max(); for (const auto & error_code_id_name : error_code_ids_) { int current_error_code; - bool found_error_code = blackboard_->get(error_code_id_name, current_error_code); - if (found_error_code) { + if (blackboard_->get(error_code_id_name, current_error_code)) { if (current_error_code != 0 && current_error_code < highest_priority_error_code) { highest_priority_error_code = current_error_code; } } else { RCLCPP_ERROR( - logger_, - "Failed to get error code: %s from blackboard", - error_code_id_name.c_str()); + logger_, + "Failed to get error code: %s from blackboard", + error_code_id_name.c_str()); } } diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index daab47b4a9e..bae65bd5196 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -3,7 +3,7 @@ geometry_msgs/PoseStamped[] poses --- #result definition int32[] missed_waypoints -int16[] error_code +int16[] error_codes --- #feedback definition uint32 current_waypoint diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 14d406717a9..39f207e2598 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -49,6 +49,12 @@ struct MissedWaypoint int error_code; }; +struct GoalStatus +{ + ActionStatus status; + int error_code; +}; + /** * @class nav2_waypoint_follower::WaypointFollower * @brief An action server that uses behavior tree for navigating a robot to its @@ -138,13 +144,11 @@ class WaypointFollower : public nav2_util::LifecycleNode ActionClient::SharedPtr nav_to_pose_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::shared_future::SharedPtr> future_goal_handle_; - int error_code_; + std::shared_future::SharedPtr> future_goal_handle_;gi bool stop_on_failure_; - ActionStatus current_goal_status_; int loop_rate_; -// std::vector failed_ids_; + GoalStatus current_goal_status_; std::vector missed_waypoints_; // Task Execution At Waypoint Plugin diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 7739a17be3b..501b688119e 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -205,14 +205,14 @@ WaypointFollower::followWaypoints() std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); - current_goal_status_ = ActionStatus::PROCESSING; + current_goal_status_.status = ActionStatus::PROCESSING; } feedback->current_waypoint = goal_index; action_server_->publish_feedback(feedback); - if (current_goal_status_ == ActionStatus::FAILED) { - missed_waypoints_.emplace_back(MissedWaypoint{goal_index, error_code_}); + if (current_goal_status_.status == ActionStatus::FAILED) { + missed_waypoints_.emplace_back(MissedWaypoint{goal_index, current_goal_status_.error_code}); if (stop_on_failure_) { RCLCPP_WARN( @@ -222,19 +222,19 @@ WaypointFollower::followWaypoints() for (const auto missed_waypoint : missed_waypoints_) { result->missed_waypoints.push_back(missed_waypoint.index); - result->error_code.push_back(missed_waypoint.error_code); + result->error_codes.push_back(missed_waypoint.error_code); } action_server_->terminate_current(result); missed_waypoints_.clear(); - error_code_ = 0; + current_goal_status_.error_code = 0; return; } else { RCLCPP_INFO( get_logger(), "Failed to process waypoint %i," " moving to next.", goal_index); } - } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { + } else if (current_goal_status_.status == ActionStatus::SUCCEEDED) { RCLCPP_INFO( get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); @@ -243,9 +243,6 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); - if (!is_task_executed) { - missed_waypoints_.emplace_back(MissedWaypoint{goal_index, error_code_}); - } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { RCLCPP_WARN( @@ -255,12 +252,12 @@ WaypointFollower::followWaypoints() for (const auto missed_waypoint : missed_waypoints_) { result->missed_waypoints.push_back(missed_waypoint.index); - result->error_code.push_back(missed_waypoint.error_code); + result->error_codes.push_back(missed_waypoint.error_code); } action_server_->terminate_current(result); missed_waypoints_.clear(); - error_code_ = 0; + current_goal_status_.error_code = 0; return; } else { RCLCPP_INFO( @@ -269,8 +266,8 @@ WaypointFollower::followWaypoints() } } - if (current_goal_status_ != ActionStatus::PROCESSING && - current_goal_status_ != ActionStatus::UNKNOWN) + if (current_goal_status_.status != ActionStatus::PROCESSING && + current_goal_status_.status != ActionStatus::UNKNOWN) { // Update server state goal_index++; @@ -282,12 +279,12 @@ WaypointFollower::followWaypoints() for (const auto missed_waypoint : missed_waypoints_) { result->missed_waypoints.push_back(missed_waypoint.index); - result->error_code.push_back(missed_waypoint.error_code); + result->error_codes.push_back(missed_waypoint.error_code); } action_server_->succeeded_current(result); missed_waypoints_.clear(); - error_code_ = 0; + current_goal_status_.error_code = 0; return; } } else { @@ -316,17 +313,17 @@ WaypointFollower::resultCallback( switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - current_goal_status_ = ActionStatus::SUCCEEDED; + current_goal_status_.status = ActionStatus::SUCCEEDED; return; case rclcpp_action::ResultCode::ABORTED: - current_goal_status_ = ActionStatus::FAILED; - error_code_ = result.result->error_code; + current_goal_status_.status = ActionStatus::FAILED; + current_goal_status_.error_code = result.result->error_code; return; case rclcpp_action::ResultCode::CANCELED: - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; return; default: - current_goal_status_ = ActionStatus::UNKNOWN; + current_goal_status_.status = ActionStatus::UNKNOWN; return; } } @@ -339,7 +336,7 @@ WaypointFollower::goalResponseCallback( RCLCPP_ERROR( get_logger(), "navigate_to_pose action client failed to send goal to server."); - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; } } From 117f2fb2ba1f5b19b6bfe211c8cd94c0efeb3c73 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sat, 12 Nov 2022 09:03:21 -0500 Subject: [PATCH 25/61] build fix --- .../include/nav2_waypoint_follower/waypoint_follower.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 39f207e2598..2164bea55a0 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -144,7 +144,7 @@ class WaypointFollower : public nav2_util::LifecycleNode ActionClient::SharedPtr nav_to_pose_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::shared_future::SharedPtr> future_goal_handle_;gi + std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; int loop_rate_; From 0516f6b3f927bce940b449c7bd31ffda4236333c Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 12:02:34 -0500 Subject: [PATCH 26/61] clean up --- .../nav2_behavior_tree/bt_action_server.hpp | 4 ++-- ...test_compute_path_through_poses_action.cpp | 19 +++++++++++++++---- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 166ce53e0e0..e8a6778f1c4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -190,9 +190,9 @@ class BtActionServer void executeCallback(); /** - * \brief updates the action server result to the highest priority error code posted on the + * @brief updates the action server result to the highest priority error code posted on the * blackboard - * \param result the action server result to be updated + * @param result the action server result to be updated */ void populateErrorCode(typename std::shared_ptr result); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 122c7646212..d297e9fb9af 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -14,9 +14,8 @@ // limitations under the License. #include + #include -#include -#include #include #include "nav_msgs/msg/path.hpp" @@ -30,6 +29,9 @@ class ComputePathThroughPosesActionServer : public TestActionServer { + using Action = nav2_msgs::action::ComputePathThroughPoses; + using ActionResult = Action::Result; + public: ComputePathThroughPosesActionServer() : TestActionServer("compute_path_through_poses") @@ -38,11 +40,11 @@ class ComputePathThroughPosesActionServer protected: void execute( const typename std::shared_ptr< - rclcpp_action::ServerGoalHandle> goal_handle) + rclcpp_action::ServerGoalHandle> goal_handle) override { const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); + auto result = std::make_shared(); result->path.poses.resize(2); result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x; if (goal->use_start) { @@ -158,6 +160,11 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); + // check if the returned error code is correct + int error_code; + config_->blackboard->get("error_code_id", error_code); + EXPECT_EQ(error_code, nav2_msgs::action::ComputePathThroughPoses::Goal::NONE); + // halt node so another goal can be sent tree_->rootNode()->halt(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); @@ -224,6 +231,10 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); + int error_code; + config_->blackboard->get("error_code_id", error_code); + EXPECT_EQ(error_code, nav2_msgs::action::ComputePathThroughPoses::Goal::NONE); + // halt node so another goal can be sent tree_->rootNode()->halt(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); From 7f57e869fa30d31c927c453df9f7d2c927763dc1 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 12:03:44 -0500 Subject: [PATCH 27/61] revert --- ...test_compute_path_through_poses_action.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index d297e9fb9af..122c7646212 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -14,8 +14,9 @@ // limitations under the License. #include - #include +#include +#include #include #include "nav_msgs/msg/path.hpp" @@ -29,9 +30,6 @@ class ComputePathThroughPosesActionServer : public TestActionServer { - using Action = nav2_msgs::action::ComputePathThroughPoses; - using ActionResult = Action::Result; - public: ComputePathThroughPosesActionServer() : TestActionServer("compute_path_through_poses") @@ -40,11 +38,11 @@ class ComputePathThroughPosesActionServer protected: void execute( const typename std::shared_ptr< - rclcpp_action::ServerGoalHandle> goal_handle) + rclcpp_action::ServerGoalHandle> goal_handle) override { const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); + auto result = std::make_shared(); result->path.poses.resize(2); result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x; if (goal->use_start) { @@ -160,11 +158,6 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); - // check if the returned error code is correct - int error_code; - config_->blackboard->get("error_code_id", error_code); - EXPECT_EQ(error_code, nav2_msgs::action::ComputePathThroughPoses::Goal::NONE); - // halt node so another goal can be sent tree_->rootNode()->halt(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); @@ -231,10 +224,6 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); - int error_code; - config_->blackboard->get("error_code_id", error_code); - EXPECT_EQ(error_code, nav2_msgs::action::ComputePathThroughPoses::Goal::NONE); - // halt node so another goal can be sent tree_->rootNode()->halt(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); From 33d44fb38e6d81e2d7cc2008a7bb62bd43bb2a3a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 12:06:03 -0500 Subject: [PATCH 28/61] space --- .../navigate_w_replanning_only_if_path_becomes_invalid.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index c9f1cce9035..4e8df5d2941 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -18,4 +18,4 @@ - \ No newline at end of file + From d08d61c6ee1bd408ef76f8c3e7f4a9c3ecb55ed1 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 12:08:44 -0500 Subject: [PATCH 29/61] remove --- .../navigate_w_replanning_only_if_path_becomes_invalid.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index 4e8df5d2941..c9f1cce9035 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -18,4 +18,4 @@ - + \ No newline at end of file From 77fc5d39c342121933eac97a5d6d2a03ed6926c8 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 12:10:48 -0500 Subject: [PATCH 30/61] try to make github happ --- .../navigate_w_replanning_only_if_path_becomes_invalid.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index c9f1cce9035..4e8df5d2941 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -18,4 +18,4 @@ - \ No newline at end of file + From 72fcadca2d6abf47a8ea19ea0fba6be6ebb76790 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 18:34:25 -0500 Subject: [PATCH 31/61] stop gap --- nav2_bringup/params/nav2_params.yaml | 15 ++- nav2_system_tests/CMakeLists.txt | 57 +++++++-- nav2_system_tests/package.xml | 2 + .../src/error_codes/CMakeLists.txt | 12 ++ .../controller/controller_error_plugins.cpp | 24 ++++ .../controller/controller_error_plugins.hpp | 114 ++++++++++++++++++ .../src/error_codes/controller_plugins.xml | 26 ++++ .../src/error_codes/error_code_param.yaml | 16 +++ .../src/error_codes/test_error_codes.py | 89 ++++++++++++++ .../error_codes/test_error_codes_launch.py | 69 +++++++++++ 10 files changed, 410 insertions(+), 14 deletions(-) create mode 100644 nav2_system_tests/src/error_codes/CMakeLists.txt create mode 100644 nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp create mode 100644 nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp create mode 100644 nav2_system_tests/src/error_codes/controller_plugins.xml create mode 100644 nav2_system_tests/src/error_codes/error_code_param.yaml create mode 100755 nav2_system_tests/src/error_codes/test_error_codes.py create mode 100755 nav2_system_tests/src/error_codes/test_error_codes_launch.py diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 8ffb148fde8..316827bbc7b 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -109,7 +109,20 @@ controller_server: failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] + controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] + unknown: + plugin: "nav2_system_tests::UnknownErrorController" + tf_error: + plugin: "nav2_system_tests::TFErrorController" + invalid_path: + plugin: "nav2_system_tests::InvalidPathErrorController" + patience_exceeded: + plugin: "nav2_system_tests::PatienceExceededErrorController" + failed_to_make_progress: + plugin: "nav2_system_tests::FailedToMakeProgressErrorController" + no_valid_control: + plugin: "nav2_system_tests::NoValidControlErrorController" + # Progress checker parameters progress_checker: diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6d56df1ef7e..84ad3588cb8 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -4,6 +4,8 @@ project(nav2_system_tests) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_map_server REQUIRED) @@ -22,12 +24,18 @@ find_package(nav2_navfn_planner REQUIRED) find_package(nav2_planner REQUIRED) find_package(navigation2) find_package(angles REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_core REQUIRED) +find_package(pluginlib REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() set(dependencies rclcpp + rclcpp_action + rclcpp_lifecycle nav2_util nav2_map_server nav2_msgs @@ -44,9 +52,30 @@ set(dependencies nav2_planner nav2_navfn_planner angles + nav2_costmap_2d + nav2_core + pluginlib + tf2_ros behaviortree_cpp_v3 ) +set(local_controller_plugin_lib local_controller) + +add_library(${local_controller_plugin_lib} SHARED src/error_codes/controller/controller_error_plugins.cpp) +ament_target_dependencies(${local_controller_plugin_lib} ${dependencies}) +target_compile_definitions(${local_controller_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/controller_plugins.xml) + +install(TARGETS ${local_controller_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + +install(FILES src/error_codes/controller_plugins.xml + DESTINATION share/${PROJECT_NAME} + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -54,21 +83,23 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) - add_subdirectory(src/behavior_tree) - add_subdirectory(src/planning) - add_subdirectory(src/localization) - add_subdirectory(src/system) - add_subdirectory(src/system_failure) - add_subdirectory(src/updown) - add_subdirectory(src/waypoint_follower) - add_subdirectory(src/behaviors/spin) - add_subdirectory(src/behaviors/wait) - add_subdirectory(src/behaviors/backup) - add_subdirectory(src/behaviors/drive_on_heading) - add_subdirectory(src/behaviors/assisted_teleop) - add_subdirectory(src/costmap_filters) +# add_subdirectory(src/behavior_tree) +# add_subdirectory(src/planning) +# add_subdirectory(src/localization) +# add_subdirectory(src/system) +# add_subdirectory(src/system_failure) +# add_subdirectory(src/updown) +# add_subdirectory(src/waypoint_follower) +# add_subdirectory(src/behaviors/spin) +# add_subdirectory(src/behaviors/wait) +# add_subdirectory(src/behaviors/backup) +# add_subdirectory(src/behaviors/drive_on_heading) +# add_subdirectory(src/behaviors/assisted_teleop) +# add_subdirectory(src/costmap_filters) + add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() +ament_export_libraries(${local_controller_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 0940173e04b..9eb6b14e9e6 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -65,5 +65,7 @@ ament_cmake + + diff --git a/nav2_system_tests/src/error_codes/CMakeLists.txt b/nav2_system_tests/src/error_codes/CMakeLists.txt new file mode 100644 index 00000000000..be7052be168 --- /dev/null +++ b/nav2_system_tests/src/error_codes/CMakeLists.txt @@ -0,0 +1,12 @@ +ament_add_test(test_error_codes + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_error_codes_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp new file mode 100644 index 00000000000..68f068fab61 --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp @@ -0,0 +1,24 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "controller_error_plugins.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::FailedToMakeProgressErrorController, + nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::PatienceExceededErrorController,nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::InvalidPathErrorController,nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidControlErrorController,nav2_core::Controller) diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp new file mode 100644 index 00000000000..81b677f9a1e --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -0,0 +1,114 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ +#define ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ + +#include +#include + +#include "nav2_core/controller.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorController : public nav2_core::Controller +{ +public: + UnknownErrorController() = default; + ~UnknownErrorController() = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() {} + + void activate() {} + + void deactivate() {} + + void setPlan(const nav_msgs::msg::Path &) {} + + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::ControllerException("Unknown Error"); + } + + void setSpeedLimit(const double &, const bool &) {} +}; + +class TFErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::ControllerTFError("TF error"); + } +}; + +class FailedToMakeProgressErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::FailedToMakeProgress("Failed to make progress"); + } +}; + +class PatienceExceededErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::PatienceExceeded("Patience exceeded"); + } +}; + +class InvalidPathErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::InvalidPath("Invalid path"); + } +}; + +class NoValidControlErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::NoValidControl("No valid control"); + } +}; + +} // namespace nav2_system_tests + +#endif // ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ diff --git a/nav2_system_tests/src/error_codes/controller_plugins.xml b/nav2_system_tests/src/error_codes/controller_plugins.xml new file mode 100644 index 00000000000..c20fe0ab5ec --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller_plugins.xml @@ -0,0 +1,26 @@ + + + This local planner produces the general exception. + + + This local planner produces a tf exception. + + + This local planner produces a failed to make progress exception. + + + This local planner produces a start patience exceeded exception. + + + This local planner produces an invalid path exception. + + + This local planner produces a no valid control exception. + + diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml new file mode 100644 index 00000000000..1eccbf45044 --- /dev/null +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -0,0 +1,16 @@ +controller_server: + ros__parameters: + controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded", + 'failed_to_make_progress', 'no_valid_control'] + unknown: + plugin: "nav2_error_code_test::UnknownErrorController" + tf_error: + plugin: "nav2_error_code_test::TFErrorController" + invalid_path: + plugin: "nav2_error_code_test::InvalidPathErrorController" + patience_exceeded: + plugin: "nav2_error_code_test::PatienceExceededErrorController" + failed_to_make_progress: + plugin: "nav2_error_code_test::FailedToMakeProgressErrorController" + no_valid_control: + plugin: "nav2_error_code_test::NoValidControlErrorController" diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py new file mode 100755 index 00000000000..4e849b15572 --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -0,0 +1,89 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import time + +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult + +import rclpy +from nav2_msgs.action import FollowPath + + +def main(argv=sys.argv[1:]): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + + # Initial pose does not respect the orientation... not sure why + initial_pose.pose.position.x = -2.00 + initial_pose.pose.position.y = -0.50 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # Follow path error codes + path = Path() + + goal_pose = initial_pose + goal_pose.pose.position.x += 0.25 + + goal_pose1 = goal_pose + goal_pose1.pose.position.x += 0.25 + + path.poses.append(initial_pose) + path.poses.append(goal_pose) + path.poses.append(goal_pose1) + + follow_path = { + 'unknown': FollowPath.Goal().UNKNOWN, + '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().PATIENCE_EXCEEDED + } + + for controller, error_code in follow_path.items(): + success = navigator.followPath(path, controller) + + if success: + while not navigator.isTaskComplete(): + time.sleep(0.5) + + assert navigator.result_future.result().result.error_code == error_code, \ + "Follow path error code does not match" + + else: + assert False, "Follow path was rejected" + + + + navigator.lifecycleShutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py new file mode 100755 index 00000000000..785bbb8943b --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -0,0 +1,69 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + +def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') + # params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') + # + # configured_params = RewrittenYaml( + # source_file=params_file, + # root_key='', + # param_rewrites='', + # convert_types=True) + # + # context = LaunchContext() + # new_yaml = configured_params.perform(context) + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')), + launch_arguments={ + 'use_rviz': 'False', + 'use_composition': 'False'}.items()) + ]) + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test_error_codes_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'test_error_codes.py')], + name='test_error_codes', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test_error_codes_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) From 3fd3c055ed7bdd8d1a3fc8a8a297c5e71660a4cc Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 19:02:05 -0500 Subject: [PATCH 32/61] loading in param file --- nav2_bringup/params/nav2_params.yaml | 110 +++--- .../src/error_codes/error_code_param.yaml | 341 +++++++++++++++++- .../src/error_codes/test_error_codes.py | 1 + .../error_codes/test_error_codes_launch.py | 8 +- 4 files changed, 386 insertions(+), 74 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 316827bbc7b..4577f495507 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -50,55 +50,52 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - error_code_id_names: - - follow_path_error_code_id - - compute_path_error_code_id + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node controller_server: ros__parameters: @@ -109,20 +106,7 @@ controller_server: failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] - unknown: - plugin: "nav2_system_tests::UnknownErrorController" - tf_error: - plugin: "nav2_system_tests::TFErrorController" - invalid_path: - plugin: "nav2_system_tests::InvalidPathErrorController" - patience_exceeded: - plugin: "nav2_system_tests::PatienceExceededErrorController" - failed_to_make_progress: - plugin: "nav2_system_tests::FailedToMakeProgressErrorController" - no_valid_control: - plugin: "nav2_system_tests::NoValidControlErrorController" - + controller_plugins: ["FollowPath"] # Progress checker parameters progress_checker: diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 1eccbf45044..c00c507a077 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -1,16 +1,341 @@ +amcl: + ros__parameters: + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + error_code_id_names: + - follow_path_error_code_id + - compute_path_error_code_id + controller_server: ros__parameters: - controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded", - 'failed_to_make_progress', 'no_valid_control'] + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.2 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] unknown: - plugin: "nav2_error_code_test::UnknownErrorController" + plugin: "nav2_system_tests::UnknownErrorController" tf_error: - plugin: "nav2_error_code_test::TFErrorController" + plugin: "nav2_system_tests::TFErrorController" invalid_path: - plugin: "nav2_error_code_test::InvalidPathErrorController" + plugin: "nav2_system_tests::InvalidPathErrorController" patience_exceeded: - plugin: "nav2_error_code_test::PatienceExceededErrorController" + plugin: "nav2_system_tests::PatienceExceededErrorController" failed_to_make_progress: - plugin: "nav2_error_code_test::FailedToMakeProgressErrorController" + plugin: "nav2_system_tests::FailedToMakeProgressErrorController" no_valid_control: - plugin: "nav2_error_code_test::NoValidControlErrorController" + plugin: "nav2_system_tests::NoValidControlErrorController" + + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # /~https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 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 4e849b15572..797d11e76fd 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -82,6 +82,7 @@ def main(argv=sys.argv[1:]): navigator.lifecycleShutdown() + rclpy.shutdown() exit(0) diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index 785bbb8943b..f59764d5601 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -30,8 +30,9 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') - # params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') - # + params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') + print(params_file) + # configured_params = RewrittenYaml( # source_file=params_file, # root_key='', @@ -47,7 +48,8 @@ def generate_launch_description(): os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')), launch_arguments={ 'use_rviz': 'False', - 'use_composition': 'False'}.items()) + 'use_composition': 'False', + 'params_file': params_file}.items()) ]) def main(argv=sys.argv[1:]): From 18b88c728d6e4d731c919e31977e74923db534c8 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 13 Nov 2022 21:25:40 -0500 Subject: [PATCH 33/61] working tests :) --- .../nav2_simple_commander/robot_navigator.py | 32 ++-- nav2_system_tests/CMakeLists.txt | 57 ++----- nav2_system_tests/package.xml | 2 +- .../controller/controller_error_plugins.cpp | 11 +- .../controller/controller_error_plugins.hpp | 30 ++-- .../src/error_codes/error_code_param.yaml | 27 +++- .../planner/planner_error_plugin.cpp | 26 ++++ .../planner/planner_error_plugin.hpp | 145 ++++++++++++++++++ .../src/error_codes/planner_plugins.xml | 42 +++++ .../src/error_codes/test_error_codes.py | 47 +++++- .../error_codes/test_error_codes_launch.py | 18 +-- 11 files changed, 337 insertions(+), 100 deletions(-) create mode 100644 nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp create mode 100644 nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp create mode 100644 nav2_system_tests/src/error_codes/planner_plugins.xml diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a844498ab83..7378fac4bd9 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -345,49 +345,59 @@ def _getPathImpl(self, start, goal, planner_id='', use_start=False): self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status - if self.status != GoalStatus.STATUS_SUCCEEDED: - self.warn(f'Getting path failed with status code: {self.status}') - return None return self.result_future.result().result def getPath(self, start, goal, planner_id='', use_start=False): """Send a `ComputePathToPose` action request.""" rtn = self._getPathImpl(start, goal, planner_id='', use_start=False) + + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn(f'Getting path failed with status code: {self.status}') + return None + if not rtn: return None else: return rtn.path - - def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): - """Send a `ComputePathThroughPoses` action request.""" + def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False): + """ + Send a `ComputePathThroughPoses` action request. + Internal implementation to get the full result, not just the path. + """ self.debug("Waiting for 'ComputePathThroughPoses' action server") while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathThroughPoses' action server not available, waiting...") - goal_msg = ComputePathThroughPoses.Goal() goal_msg.start = start goal_msg.goals = goals goal_msg.planner_id = planner_id goal_msg.use_start = use_start - self.info('Getting path...') send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() - if not self.goal_handle.accepted: self.error('Get path was rejected!') return None - self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status + + return self.result_future.result().result + + def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): + """Send a `ComputePathThroughPoses` action request.""" + rtn = self.__getPathThroughPosesImpl(start, goals, planner_id='', use_start=False) + if self.status != GoalStatus.STATUS_SUCCEEDED: self.warn(f'Getting path failed with status code: {self.status}') return None - return self.result_future.result().result.path + if not rtn: + return None + else: + return rtn.path def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): """ diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 84ad3588cb8..6d56df1ef7e 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -4,8 +4,6 @@ project(nav2_system_tests) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_map_server REQUIRED) @@ -24,18 +22,12 @@ find_package(nav2_navfn_planner REQUIRED) find_package(nav2_planner REQUIRED) find_package(navigation2) find_package(angles REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_core REQUIRED) -find_package(pluginlib REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) -find_package(tf2_ros REQUIRED) nav2_package() set(dependencies rclcpp - rclcpp_action - rclcpp_lifecycle nav2_util nav2_map_server nav2_msgs @@ -52,30 +44,9 @@ set(dependencies nav2_planner nav2_navfn_planner angles - nav2_costmap_2d - nav2_core - pluginlib - tf2_ros behaviortree_cpp_v3 ) -set(local_controller_plugin_lib local_controller) - -add_library(${local_controller_plugin_lib} SHARED src/error_codes/controller/controller_error_plugins.cpp) -ament_target_dependencies(${local_controller_plugin_lib} ${dependencies}) -target_compile_definitions(${local_controller_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -pluginlib_export_plugin_description_file(nav2_core src/error_codes/controller_plugins.xml) - -install(TARGETS ${local_controller_plugin_lib} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) - -install(FILES src/error_codes/controller_plugins.xml - DESTINATION share/${PROJECT_NAME} - ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -83,23 +54,21 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) -# add_subdirectory(src/behavior_tree) -# add_subdirectory(src/planning) -# add_subdirectory(src/localization) -# add_subdirectory(src/system) -# add_subdirectory(src/system_failure) -# add_subdirectory(src/updown) -# add_subdirectory(src/waypoint_follower) -# add_subdirectory(src/behaviors/spin) -# add_subdirectory(src/behaviors/wait) -# add_subdirectory(src/behaviors/backup) -# add_subdirectory(src/behaviors/drive_on_heading) -# add_subdirectory(src/behaviors/assisted_teleop) -# add_subdirectory(src/costmap_filters) - add_subdirectory(src/error_codes) + add_subdirectory(src/behavior_tree) + add_subdirectory(src/planning) + add_subdirectory(src/localization) + add_subdirectory(src/system) + add_subdirectory(src/system_failure) + add_subdirectory(src/updown) + add_subdirectory(src/waypoint_follower) + add_subdirectory(src/behaviors/spin) + add_subdirectory(src/behaviors/wait) + add_subdirectory(src/behaviors/backup) + add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/behaviors/assisted_teleop) + add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() -ament_export_libraries(${local_controller_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 9eb6b14e9e6..12f269a2d9a 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -65,7 +65,7 @@ ament_cmake - + diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp index 68f068fab61..efcdac9c82b 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp @@ -17,8 +17,9 @@ #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorController, nav2_core::Controller) PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorController, nav2_core::Controller) -PLUGINLIB_EXPORT_CLASS(nav2_system_tests::FailedToMakeProgressErrorController, - nav2_core::Controller) -PLUGINLIB_EXPORT_CLASS(nav2_system_tests::PatienceExceededErrorController,nav2_core::Controller) -PLUGINLIB_EXPORT_CLASS(nav2_system_tests::InvalidPathErrorController,nav2_core::Controller) -PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidControlErrorController,nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS( + nav2_system_tests::FailedToMakeProgressErrorController, + nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::PatienceExceededErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::InvalidPathErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidControlErrorController, nav2_core::Controller) diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp index 81b677f9a1e..ba194b195d8 100644 --- a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -57,9 +57,9 @@ class UnknownErrorController : public nav2_core::Controller class TFErrorController : public UnknownErrorController { virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( - const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) { throw nav2_core::ControllerTFError("TF error"); } @@ -68,9 +68,9 @@ class TFErrorController : public UnknownErrorController class FailedToMakeProgressErrorController : public UnknownErrorController { virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( - const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) { throw nav2_core::FailedToMakeProgress("Failed to make progress"); } @@ -79,9 +79,9 @@ class FailedToMakeProgressErrorController : public UnknownErrorController class PatienceExceededErrorController : public UnknownErrorController { virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( - const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) { throw nav2_core::PatienceExceeded("Patience exceeded"); } @@ -90,9 +90,9 @@ class PatienceExceededErrorController : public UnknownErrorController class InvalidPathErrorController : public UnknownErrorController { virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( - const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) { throw nav2_core::InvalidPath("Invalid path"); } @@ -101,9 +101,9 @@ class InvalidPathErrorController : public UnknownErrorController class NoValidControlErrorController : public UnknownErrorController { virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( - const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::Twist &, - nav2_core::GoalChecker *) + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) { throw nav2_core::NoValidControl("No valid control"); } diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index c00c507a077..2768152ea68 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -276,12 +276,27 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + planner_plugins: [ "unknown", "tf_error", "start_outside_map", "goal_outside_map", + "start_occupied", "goal_occupied", "timeout","no_valid_path", + "no_viapoints_given" ] + unknown: + plugin: "nav2_system_tests::UnknownErrorPlanner" + tf_error: + plugin: "nav2_system_tests::TFErrorPlanner" + start_outside_map: + plugin: "nav2_system_tests::StartOutsideMapErrorPlanner" + goal_outside_map: + plugin: "nav2_system_tests::GoalOutsideMapErrorPlanner" + start_occupied: + plugin: "nav2_system_tests::StartOccupiedErrorPlanner" + goal_occupied: + plugin: "nav2_system_tests::GoalOccupiedErrorPlanner" + timeout: + plugin: "nav2_system_tests::TimedOutErrorPlanner" + no_valid_path: + plugin: "nav2_system_tests::NoValidPathErrorPlanner" + no_viapoints_given: + plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" smoother_server: ros__parameters: diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp new file mode 100644 index 00000000000..90b18c5b117 --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "planner_error_plugin.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::StartOccupiedErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::GoalOccupiedErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::StartOutsideMapErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::GoalOutsideMapErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidPathErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimedOutErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoViapointsGivenErrorPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp new file mode 100644 index 00000000000..4b2919d1adc --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -0,0 +1,145 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ +#define ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_core/planner_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorPlanner : public nav2_core::GlobalPlanner +{ +public: + UnknownErrorPlanner() = default; + ~UnknownErrorPlanner() = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() override {} + + void activate() override {} + + void deactivate() override {} + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerException("Unknown Error"); + } +}; + + +class StartOccupiedErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::StartOccupied("Start Occupied"); + } +}; + +class GoalOccupiedErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::GoalOccupied("Goal occupied"); + } +}; + +class StartOutsideMapErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::StartOutsideMapBounds("Start OutsideMapBounds"); + } +}; + +class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } +}; + +class NoValidPathErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::NoValidPathCouldBeFound("No valid path could be found"); + } +}; + + +class TimedOutErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerTimedOut("Planner Timed Out"); + } +}; + +class TFErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerTFError("TF Error"); + } +}; + +class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::NoViapointsGiven("No Via points given"); + } +}; + +} // namespace nav2_system_tests + + +#endif // ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ diff --git a/nav2_system_tests/src/error_codes/planner_plugins.xml b/nav2_system_tests/src/error_codes/planner_plugins.xml new file mode 100644 index 00000000000..9d6f7f0545b --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner_plugins.xml @@ -0,0 +1,42 @@ + + + This global planner produces a goal a timeout exception. + + + This global planner produces a start occupied exception. + + + This global planner produces a goal occupied exception. + + + This global planner produces a start outside map bounds exception. + + + This global planner produces a goal outside map bounds exception. + + + This global planner produces a no valid path exception. + + + This global planner produces a no via points given exception. + + + This global planner produces a timed out exception. + + + This global planner produces a planner tf error exception. + + + This global planner produces a no via points exception. + + 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 797d11e76fd..ece5dc6b8d5 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -18,10 +18,10 @@ from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Path -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator import rclpy -from nav2_msgs.action import FollowPath +from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses def main(argv=sys.argv[1:]): @@ -79,7 +79,48 @@ def main(argv=sys.argv[1:]): else: assert False, "Follow path was rejected" - + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + + # Check compute path to pose error codes + goal_pose.pose.position.x = -1.00 + goal_pose.pose.position.y = -0.50 + goal_pose.pose.orientation.z = 0.0 + goal_pose.pose.orientation.w = 1.0 + + compute_path_to_pose = { + 'unknown': ComputePathToPose.Goal().UNKNOWN, + '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} + + for planner, error_code in compute_path_to_pose.items(): + result = navigator._getPathImpl(initial_pose, goal_pose, planner) + assert result.error_code == error_code, "Compute path to pose error does not match" + + # Check compute path through error codes + goal_pose1 = goal_pose + goal_poses = [goal_pose, goal_pose1] + + compute_path_through_poses = { + 'unknown': ComputePathThroughPoses.Goal().UNKNOWN, + '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} + + for planner, error_code in compute_path_through_poses.items(): + result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) + assert result.error_code == error_code, "Compute path through pose error does not match" navigator.lifecycleShutdown() rclpy.shutdown() diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index f59764d5601..7056a946f67 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -20,27 +20,14 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable -from launch.launch_context import LaunchContext -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node +from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch_testing.legacy import LaunchTestService +from launch.launch_description_sources import PythonLaunchDescriptionSource -from nav2_common.launch import RewrittenYaml def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') - print(params_file) - - # configured_params = RewrittenYaml( - # source_file=params_file, - # root_key='', - # param_rewrites='', - # convert_types=True) - # - # context = LaunchContext() - # new_yaml = configured_params.perform(context) return LaunchDescription([ IncludeLaunchDescription( @@ -52,6 +39,7 @@ def generate_launch_description(): 'params_file': params_file}.items()) ]) + def main(argv=sys.argv[1:]): ld = generate_launch_description() From 6554022fbb3f9667693e1a6e1ae8fa81a65f898e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 09:50:35 -0500 Subject: [PATCH 34/61] lint --- nav2_simple_commander/nav2_simple_commander/robot_navigator.py | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 7378fac4bd9..d075f001569 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -360,6 +360,7 @@ def getPath(self, start, goal, planner_id='', use_start=False): return None else: return rtn.path + def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False): """ Send a `ComputePathThroughPoses` action request. From 153f9cedda0bddb37bb02df197d9674683d8263b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 10:18:34 -0500 Subject: [PATCH 35/61] fixed cmake --- nav2_system_tests/CMakeLists.txt | 35 ++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6d56df1ef7e..86f5d7cf01e 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -47,6 +47,40 @@ set(dependencies behaviortree_cpp_v3 ) +set(local_controller_plugin_lib local_controller) + +add_library(${local_controller_plugin_lib} SHARED src/error_codes/controller/controller_error_plugins.cpp) +ament_target_dependencies(${local_controller_plugin_lib} ${dependencies}) +target_compile_definitions(${local_controller_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/controller_plugins.xml) + +install(TARGETS ${local_controller_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/controller_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + +set(global_planner_plugin_lib global_planner) + +add_library(${global_planner_plugin_lib} SHARED src/error_codes/planner/planner_error_plugin.cpp) +ament_target_dependencies(${global_planner_plugin_lib} ${dependencies}) +target_compile_definitions(${global_planner_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/planner_plugins.xml) + +install(TARGETS ${global_planner_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/planner_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -67,6 +101,7 @@ if(BUILD_TESTING) add_subdirectory(src/behaviors/drive_on_heading) add_subdirectory(src/behaviors/assisted_teleop) add_subdirectory(src/costmap_filters) + add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() From 6c4c602aacaf10b34cacf86511d6f40acba894a8 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 11:14:45 -0500 Subject: [PATCH 36/61] lint --- nav2_simple_commander/nav2_simple_commander/robot_navigator.py | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index d075f001569..f1d9633d4c4 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -364,6 +364,7 @@ def getPath(self, start, goal, planner_id='', use_start=False): def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False): """ Send a `ComputePathThroughPoses` action request. + Internal implementation to get the full result, not just the path. """ self.debug("Waiting for 'ComputePathThroughPoses' action server") From b69225fbac352b68f0c8e76a3ca5bfe8cf7e46ff Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 12:02:24 -0500 Subject: [PATCH 37/61] lint --- nav2_simple_commander/nav2_simple_commander/robot_navigator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index f1d9633d4c4..212b4253bec 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -364,7 +364,7 @@ def getPath(self, start, goal, planner_id='', use_start=False): def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False): """ Send a `ComputePathThroughPoses` action request. - + Internal implementation to get the full result, not just the path. """ self.debug("Waiting for 'ComputePathThroughPoses' action server") From c0b8b7a4340bd47597937209bd7e7ca202ec0d45 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 12:16:55 -0500 Subject: [PATCH 38/61] trigger build --- nav2_msgs/action/ComputePathToPose.action | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 57dca475b1f..b3d87699a58 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -21,4 +21,3 @@ nav_msgs/Path path builtin_interfaces/Duration planning_time int16 error_code --- -#feedback definition From 6d956ff24fe2047853e8e46de974e4c84eda3a88 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 15:38:24 -0500 Subject: [PATCH 39/61] added invalid plugin error --- nav2_controller/src/controller_server.cpp | 9 ++++++- .../nav2_core/controller_exceptions.hpp | 7 +++++ .../include/nav2_core/planner_exceptions.hpp | 7 +++++ .../action/ComputePathThroughPoses.action | 17 ++++++------ nav2_msgs/action/ComputePathToPose.action | 15 ++++++----- nav2_msgs/action/FollowPath.action | 11 ++++---- nav2_planner/src/planner_server.cpp | 13 ++++++++-- nav2_system_tests/CMakeLists.txt | 26 +++++++++---------- .../planner/planner_error_plugin.hpp | 1 - .../src/error_codes/test_error_codes.py | 3 +++ 10 files changed, 72 insertions(+), 37 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c527e41b146..9d341394a62 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -353,7 +353,7 @@ void ControllerServer::computeControl() if (findControllerId(c_name, current_controller)) { current_controller_ = current_controller; } else { - throw nav2_core::ControllerException("Failed to find controller name: " + c_name); + throw nav2_core::InvalidController("Failed to find controller name: " + c_name); } std::string gc_name = action_server_->get_current_goal()->goal_checker_id; @@ -403,6 +403,13 @@ void ControllerServer::computeControl() controller_frequency_); } } + } catch (nav2_core::InvalidController & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::INVALID_CONTROLLER; + action_server_->terminate_current(result); + return; } catch (nav2_core::ControllerTFError & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp index bf9c22f4e6e..5f87c4c2877 100644 --- a/nav2_core/include/nav2_core/controller_exceptions.hpp +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -27,6 +27,13 @@ class ControllerException : public std::runtime_error : std::runtime_error(description) {} }; +class InvalidController : public ControllerException +{ +public: + explicit InvalidController(const std::string & description) + : ControllerException(description) {} +}; + class ControllerTFError : public ControllerException { public: diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index e607d98ccc7..2680b3c6695 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -50,6 +50,13 @@ class PlannerException : public std::runtime_error : std::runtime_error(description) {} }; +class InvalidPlanner : public PlannerException +{ +public: + explicit InvalidPlanner(const std::string & description) + : PlannerException(description) {} +}; + class StartOccupied : public PlannerException { public: diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 4b682dae9b6..ae1e3db8ed2 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -2,14 +2,15 @@ # Note: The expected priority order of the errors should match the message order int16 NONE=0 int16 UNKNOWN=3000 -int16 TF_ERROR=3001 -int16 START_OUTSIDE_MAP=3002 -int16 GOAL_OUTSIDE_MAP=3003 -int16 START_OCCUPIED=3004 -int16 GOAL_OCCUPIED=3005 -int16 TIMEOUT=3006 -int16 NO_VALID_PATH=3007 -int16 NO_VIAPOINTS_GIVEN=3008 +int16 INVALID_PLANNER=3001 +int16 TF_ERROR=3002 +int16 START_OUTSIDE_MAP=3003 +int16 GOAL_OUTSIDE_MAP=3004 +int16 START_OCCUPIED=3005 +int16 GOAL_OCCUPIED=3006 +int16 TIMEOUT=3007 +int16 NO_VALID_PATH=3008 +int16 NO_VIAPOINTS_GIVEN=3009 #goal definition geometry_msgs/PoseStamped[] goals diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index b3d87699a58..9e78484d045 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -2,13 +2,14 @@ # Note: The expected priority order of the errors should match the message order int16 NONE=0 int16 UNKNOWN=2000 -int16 TF_ERROR=2001 -int16 START_OUTSIDE_MAP=2002 -int16 GOAL_OUTSIDE_MAP=2003 -int16 START_OCCUPIED=2004 -int16 GOAL_OCCUPIED=2005 -int16 TIMEOUT=2006 -int16 NO_VALID_PATH=2007 +int16 INVALID_PLANNER=2001 +int16 TF_ERROR=2002 +int16 START_OUTSIDE_MAP=2003 +int16 GOAL_OUTSIDE_MAP=2004 +int16 START_OCCUPIED=2005 +int16 GOAL_OCCUPIED=2006 +int16 TIMEOUT=2007 +int16 NO_VALID_PATH=2008 #goal definition geometry_msgs/PoseStamped goal diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index f345a4e0d2e..3b839901d67 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -2,11 +2,12 @@ # Note: The expected priority order of the errors should match the message order int16 NONE=0 int16 UNKNOWN=1000 -int16 TF_ERROR=1001 -int16 INVALID_PATH=1002 -int16 PATIENCE_EXCEEDED=1003 -int16 FAILED_TO_MAKE_PROGRESS=1004 -int16 NO_VALID_CONTROL=1005 +int16 INVALID_CONTROLLER=1001 +int16 TF_ERROR=1002 +int16 INVALID_PATH=1003 +int16 PATIENCE_EXCEEDED=1004 +int16 FAILED_TO_MAKE_PROGRESS=1005 +int16 NO_VALID_CONTROL=1006 #goal definition nav_msgs/Path path diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e597522430f..f58bdac31fd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -381,7 +381,7 @@ void PlannerServer::computePlanThroughPoses() nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } // Concatenate paths together @@ -405,6 +405,10 @@ 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; + 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; @@ -480,7 +484,7 @@ PlannerServer::computePlan() result->path = getPlan(start, goal_pose, goal->planner_id); if (!validatePath(goal_pose, result->path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } // Publish the plan for visualization purposes @@ -496,6 +500,10 @@ PlannerServer::computePlan() 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } 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; + 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; @@ -556,6 +564,7 @@ PlannerServer::getPlan( get_logger(), "planner %s is not a valid planner. " "Planner names are: %s", planner_id.c_str(), planner_ids_concat_.c_str()); + throw nav2_core::InvalidPlanner("Planner id " + planner_id + " is invalid"); } } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 86f5d7cf01e..6020597edff 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -88,19 +88,19 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) - add_subdirectory(src/behavior_tree) - add_subdirectory(src/planning) - add_subdirectory(src/localization) - add_subdirectory(src/system) - add_subdirectory(src/system_failure) - add_subdirectory(src/updown) - add_subdirectory(src/waypoint_follower) - add_subdirectory(src/behaviors/spin) - add_subdirectory(src/behaviors/wait) - add_subdirectory(src/behaviors/backup) - add_subdirectory(src/behaviors/drive_on_heading) - add_subdirectory(src/behaviors/assisted_teleop) - add_subdirectory(src/costmap_filters) +# add_subdirectory(src/behavior_tree) +# add_subdirectory(src/planning) +# add_subdirectory(src/localization) +# add_subdirectory(src/system) +# add_subdirectory(src/system_failure) +# add_subdirectory(src/updown) +# add_subdirectory(src/waypoint_follower) +# add_subdirectory(src/behaviors/spin) +# add_subdirectory(src/behaviors/wait) +# add_subdirectory(src/behaviors/backup) +# add_subdirectory(src/behaviors/drive_on_heading) +# add_subdirectory(src/behaviors/assisted_teleop) +# add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index 4b2919d1adc..aea388c1655 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -57,7 +57,6 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner } }; - class StartOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( 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 ece5dc6b8d5..6ce9179607a 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -59,6 +59,7 @@ def main(argv=sys.argv[1:]): 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, @@ -91,6 +92,7 @@ def main(argv=sys.argv[1:]): 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, @@ -109,6 +111,7 @@ def main(argv=sys.argv[1:]): 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, From 240eb27325db30d4a6a8618da36bb3a98e08ac46 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 16:57:04 -0500 Subject: [PATCH 40/61] added test for piping up error codes --- .../nav2_behavior_tree/bt_action_server_impl.hpp | 13 +++++++------ nav2_system_tests/CMakeLists.txt | 2 ++ .../src/error_codes/test_error_codes.py | 14 ++++++++++++++ 3 files changed, 23 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 27ce5ac76a3..6aba308faf1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -276,18 +276,19 @@ void BtActionServer::populateErrorCode( typename std::shared_ptr result) { int highest_priority_error_code = std::numeric_limits::max(); - for (const auto & error_code_id_name : error_code_ids_) { - int current_error_code; - - if (blackboard_->get(error_code_id_name, current_error_code)) { + for (const auto & error_code_id : error_code_ids_) { + // TODO(jwallace42): Using try catch due to + // /~https://github.com/BehaviorTree/BehaviorTree.CPP/issues/271 + try { + int current_error_code = blackboard_->get(error_code_id); if (current_error_code != 0 && current_error_code < highest_priority_error_code) { highest_priority_error_code = current_error_code; } - } else { + } catch (...) { RCLCPP_ERROR( logger_, "Failed to get error code: %s from blackboard", - error_code_id_name.c_str()); + error_code_id.c_str()); } } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6020597edff..9d64aaf00c0 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -106,4 +106,6 @@ if(BUILD_TESTING) endif() +ament_export_libraries(${local_controller_plugin_lib}) +ament_export_libraries(${global_planner_plugin_lib}) ament_package() 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 6ce9179607a..66b11db3f3c 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -15,6 +15,7 @@ import sys import time +import os from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Path @@ -22,6 +23,7 @@ import rclpy from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses +from ament_index_python.packages import get_package_share_directory def main(argv=sys.argv[1:]): @@ -125,6 +127,18 @@ def main(argv=sys.argv[1:]): result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) assert result.error_code == error_code, "Compute path through pose error does not match" + # Check that the error codes are piped up correctly + behavior_tree = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + 'navigate_w_replanning_time.xml') + + success = navigator.goToPose(goal_pose, behavior_tree) + if success: + while not navigator.isTaskComplete(): + time.sleep(0.5) + assert navigator.result_future.result().result.error_code, \ + ComputePathToPose.Goal().INVALID_PLANNER + navigator.lifecycleShutdown() rclpy.shutdown() exit(0) From 18d5993a241f86c6eae1532af17626e8c20f34c6 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 17:00:12 -0500 Subject: [PATCH 41/61] clean up --- nav2_msgs/action/ComputePathToPose.action | 1 + nav2_system_tests/CMakeLists.txt | 26 +++++++++---------- .../src/waypoint_follower/tester.py | 2 +- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 9e78484d045..93a0171098a 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -22,3 +22,4 @@ nav_msgs/Path path builtin_interfaces/Duration planning_time int16 error_code --- +#feedback definition diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 9d64aaf00c0..558ae809bd4 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -88,19 +88,19 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) -# add_subdirectory(src/behavior_tree) -# add_subdirectory(src/planning) -# add_subdirectory(src/localization) -# add_subdirectory(src/system) -# add_subdirectory(src/system_failure) -# add_subdirectory(src/updown) -# add_subdirectory(src/waypoint_follower) -# add_subdirectory(src/behaviors/spin) -# add_subdirectory(src/behaviors/wait) -# add_subdirectory(src/behaviors/backup) -# add_subdirectory(src/behaviors/drive_on_heading) -# add_subdirectory(src/behaviors/assisted_teleop) -# add_subdirectory(src/costmap_filters) + add_subdirectory(src/behavior_tree) + add_subdirectory(src/planning) + add_subdirectory(src/localization) + add_subdirectory(src/system) + add_subdirectory(src/system_failure) + add_subdirectory(src/updown) + add_subdirectory(src/waypoint_follower) + add_subdirectory(src/behaviors/spin) + add_subdirectory(src/behaviors/wait) + add_subdirectory(src/behaviors/backup) + add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/behaviors/assisted_teleop) + add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 4f0ce014c4f..bedc0429a59 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -208,7 +208,7 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() - # Set a waypoint outside the bounds of the map + # a failure case time.sleep(2) test.setWaypoints([[100.0, 100.0]]) result = test.run(True) From ea70be71ecd3284b7b4ad8db10f6480942d100f5 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 17:27:23 -0500 Subject: [PATCH 42/61] test waypoint follower --- nav2_msgs/action/FollowWaypoints.action | 6 +++++ nav2_system_tests/CMakeLists.txt | 26 +++++++++---------- .../src/error_codes/test_error_codes.py | 7 +++++ .../src/waypoint_follower.cpp | 4 +++ 4 files changed, 30 insertions(+), 13 deletions(-) diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index bae65bd5196..218328cbedd 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,3 +1,9 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +int16 NONE=0 +int16 UNKNOWN=6000 +int16 TASK_EXECUTOR_FAILED=6001 + #goal definition geometry_msgs/PoseStamped[] poses --- diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 558ae809bd4..9d64aaf00c0 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -88,19 +88,19 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) - add_subdirectory(src/behavior_tree) - add_subdirectory(src/planning) - add_subdirectory(src/localization) - add_subdirectory(src/system) - add_subdirectory(src/system_failure) - add_subdirectory(src/updown) - add_subdirectory(src/waypoint_follower) - add_subdirectory(src/behaviors/spin) - add_subdirectory(src/behaviors/wait) - add_subdirectory(src/behaviors/backup) - add_subdirectory(src/behaviors/drive_on_heading) - add_subdirectory(src/behaviors/assisted_teleop) - add_subdirectory(src/costmap_filters) +# add_subdirectory(src/behavior_tree) +# add_subdirectory(src/planning) +# add_subdirectory(src/localization) +# add_subdirectory(src/system) +# add_subdirectory(src/system_failure) +# add_subdirectory(src/updown) +# add_subdirectory(src/waypoint_follower) +# add_subdirectory(src/behaviors/spin) +# add_subdirectory(src/behaviors/wait) +# add_subdirectory(src/behaviors/backup) +# add_subdirectory(src/behaviors/drive_on_heading) +# add_subdirectory(src/behaviors/assisted_teleop) +# add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) 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 66b11db3f3c..38495811086 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -139,6 +139,13 @@ def main(argv=sys.argv[1:]): assert navigator.result_future.result().result.error_code, \ ComputePathToPose.Goal().INVALID_PLANNER + success = navigator.followWaypoints(goal_poses) + if success: + while not navigator.isTaskComplete(): + time.sleep(0.5) + assert navigator.result_future.result().result.error_codes[0], \ + ComputePathToPose.Goal().INVALID_PLANNER + navigator.lifecycleShutdown() rclpy.shutdown() exit(0) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 501b688119e..48be6c67ee0 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -243,6 +243,10 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); + + if (!is_task_executed) { + missed_waypoints_.emplace_back(MissedWaypoint{goal_index, current_goal_status_.error_code}); + } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { RCLCPP_WARN( From a6f73dcc0d12f1ca1f23d1d51cbaf7c31785f864 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 19:29:31 -0500 Subject: [PATCH 43/61] only launch what is needed --- .../src/error_codes/error_code_param.yaml | 185 +----------------- .../src/error_codes/test_error_codes.py | 29 +-- .../error_codes/test_error_codes_launch.py | 64 ++++-- 3 files changed, 54 insertions(+), 224 deletions(-) diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 2768152ea68..4c3e8395204 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -1,112 +1,10 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "nav2_amcl::DifferentialMotionModel" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -bt_navigator: - ros__parameters: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - error_code_id_names: - - follow_path_error_code_id - - compute_path_error_code_id - controller_server: ros__parameters: controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.2 - failure_tolerance: 0.3 + failure_tolerance: -0.1 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] @@ -235,7 +133,7 @@ global_costmap: robot_radius: 0.22 resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + plugins: ["obstacle_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -250,28 +148,6 @@ global_costmap: raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - always_send_full_costmap: True - -# The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py -# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" - -map_saver: - ros__parameters: - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True planner_server: ros__parameters: @@ -297,60 +173,3 @@ planner_server: plugin: "nav2_system_tests::NoValidPathErrorPlanner" no_viapoints_given: plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" - -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] - spin: - plugin: "nav2_behaviors/Spin" - backup: - plugin: "nav2_behaviors/BackUp" - drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" - wait: - plugin: "nav2_behaviors/Wait" - assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 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 38495811086..13a36f95553 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -15,7 +15,6 @@ import sys import time -import os from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Path @@ -23,14 +22,13 @@ import rclpy from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses -from ament_index_python.packages import get_package_share_directory def main(argv=sys.argv[1:]): rclpy.init() navigator = BasicNavigator() - + # # Set our demo's initial pose initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' @@ -41,10 +39,6 @@ def main(argv=sys.argv[1:]): initial_pose.pose.position.y = -0.50 initial_pose.pose.orientation.z = 0.0 initial_pose.pose.orientation.w = 1.0 - navigator.setInitialPose(initial_pose) - - # Wait for navigation to fully activate, since autostarting nav2 - navigator.waitUntilNav2Active() # Follow path error codes path = Path() @@ -66,7 +60,7 @@ def main(argv=sys.argv[1:]): '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().PATIENCE_EXCEEDED + 'no_valid_control': FollowPath.Goal().NO_VALID_CONTROL } for controller, error_code in follow_path.items(): @@ -127,25 +121,6 @@ def main(argv=sys.argv[1:]): result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) assert result.error_code == error_code, "Compute path through pose error does not match" - # Check that the error codes are piped up correctly - behavior_tree = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - 'navigate_w_replanning_time.xml') - - success = navigator.goToPose(goal_pose, behavior_tree) - if success: - while not navigator.isTaskComplete(): - time.sleep(0.5) - assert navigator.result_future.result().result.error_code, \ - ComputePathToPose.Goal().INVALID_PLANNER - - success = navigator.followWaypoints(goal_poses) - if success: - while not navigator.isTaskComplete(): - time.sleep(0.5) - assert navigator.result_future.result().result.error_codes[0], \ - ComputePathToPose.Goal().INVALID_PLANNER - navigator.lifecycleShutdown() rclpy.shutdown() exit(0) diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index 7056a946f67..8594c8c7893 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -16,28 +16,64 @@ import os import sys -from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.actions import ExecuteProcess from launch_testing.legacy import LaunchTestService -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch.actions import GroupAction def generate_launch_description(): - bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')), - launch_arguments={ - 'use_rviz': 'False', - 'use_composition': 'False', - 'params_file': params_file}.items()) - ]) + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + lifecycle_nodes = ['controller_server', + 'planner_server'] + + load_nodes = GroupAction( + actions=[ + # SetParameter("use_sim_time", 'True'), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link']), + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'autostart': True}, + {'node_names': lifecycle_nodes}]) + ]) + + ld = LaunchDescription() + ld.add_action(load_nodes) + + return ld def main(argv=sys.argv[1:]): From b970f4a8a402dee4cf48ddc6dc0a31e00f60c6b2 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 19:50:53 -0500 Subject: [PATCH 44/61] waypoint test --- nav2_system_tests/CMakeLists.txt | 4 ++-- nav2_system_tests/src/error_codes/test_error_codes.py | 2 +- nav2_system_tests/src/waypoint_follower/tester.py | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 9d64aaf00c0..62043869caf 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -94,14 +94,14 @@ if(BUILD_TESTING) # add_subdirectory(src/system) # add_subdirectory(src/system_failure) # add_subdirectory(src/updown) -# add_subdirectory(src/waypoint_follower) + add_subdirectory(src/waypoint_follower) # add_subdirectory(src/behaviors/spin) # add_subdirectory(src/behaviors/wait) # add_subdirectory(src/behaviors/backup) # add_subdirectory(src/behaviors/drive_on_heading) # add_subdirectory(src/behaviors/assisted_teleop) # add_subdirectory(src/costmap_filters) - add_subdirectory(src/error_codes) +# add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() 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 13a36f95553..3079e3a3fc1 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -28,7 +28,7 @@ def main(argv=sys.argv[1:]): rclpy.init() navigator = BasicNavigator() - # + # Set our demo's initial pose initial_pose = PoseStamped() initial_pose.header.frame_id = 'map' diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index bedc0429a59..cfe49507827 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -208,12 +208,13 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() - # a failure case + # set waypoint outside of map time.sleep(2) test.setWaypoints([[100.0, 100.0]]) result = test.run(True) assert not result result = not result + assert len(test.result.error_codes) > 0 test.shutdown() test.info_msg('Done Shutting Down.') From 7b5e7d8db21bf1d1ba2a1e21646b65d02e9d6aea Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 14 Nov 2022 19:52:51 -0500 Subject: [PATCH 45/61] revert lines for robot navigator --- .../nav2_simple_commander/robot_navigator.py | 4 +++ nav2_system_tests/CMakeLists.txt | 26 +++++++++---------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 212b4253bec..5b00c6a49ea 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -370,18 +370,22 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) self.debug("Waiting for 'ComputePathThroughPoses' action server") while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathThroughPoses' action server not available, waiting...") + goal_msg = ComputePathThroughPoses.Goal() goal_msg.start = start goal_msg.goals = goals goal_msg.planner_id = planner_id goal_msg.use_start = use_start + self.info('Getting path...') send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() + if not self.goal_handle.accepted: self.error('Get path was rejected!') return None + self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 62043869caf..558ae809bd4 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -88,20 +88,20 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) -# add_subdirectory(src/behavior_tree) -# add_subdirectory(src/planning) -# add_subdirectory(src/localization) -# add_subdirectory(src/system) -# add_subdirectory(src/system_failure) -# add_subdirectory(src/updown) + add_subdirectory(src/behavior_tree) + add_subdirectory(src/planning) + add_subdirectory(src/localization) + add_subdirectory(src/system) + add_subdirectory(src/system_failure) + add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) -# add_subdirectory(src/behaviors/spin) -# add_subdirectory(src/behaviors/wait) -# add_subdirectory(src/behaviors/backup) -# add_subdirectory(src/behaviors/drive_on_heading) -# add_subdirectory(src/behaviors/assisted_teleop) -# add_subdirectory(src/costmap_filters) -# add_subdirectory(src/error_codes) + add_subdirectory(src/behaviors/spin) + add_subdirectory(src/behaviors/wait) + add_subdirectory(src/behaviors/backup) + add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/behaviors/assisted_teleop) + add_subdirectory(src/costmap_filters) + add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() From cf999b957bbd107f7860d1fe97ea4b7b03d283b0 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 10:33:15 -0500 Subject: [PATCH 46/61] fix test --- nav2_system_tests/src/planning/test_planner_plugins.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index a56a7216b13..906e8f44d3a 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -21,6 +21,7 @@ #include "planner_tester.hpp" #include "nav2_util/lifecycle_utils.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_core/planner_exceptions.hpp" using namespace std::chrono_literals; @@ -129,8 +130,12 @@ TEST(testPluginMap, Failures) auto path = obj->getPlan(start, goal, plugin_none); EXPECT_EQ(path.header.frame_id, std::string("map")); - path = obj->getPlan(start, goal, plugin_fake); - EXPECT_EQ(path.poses.size(), 0ul); + try { + path = obj->getPlan(start, goal, plugin_fake); + FAIL() << "Failed to throw invalid planner id exception"; + } catch (const nav2_core::InvalidPlanner & ex) { + EXPECT_EQ(ex.what(), std::string("Planner id fake is invalid")); + } obj->onCleanup(state); } From 6b372adab817842e59bcd6d4faae56acc6ec7253 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 10:42:59 -0500 Subject: [PATCH 47/61] waypoint test --- nav2_system_tests/src/waypoint_follower/tester.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index cfe49507827..40ce8e35746 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -18,7 +18,7 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped -from nav2_msgs.action import FollowWaypoints +from nav2_msgs.action import FollowWaypoints, ComputePathToPose from nav2_msgs.srv import ManageLifecycleNodes import rclpy @@ -214,7 +214,8 @@ def main(argv=sys.argv[1:]): result = test.run(True) assert not result result = not result - assert len(test.result.error_codes) > 0 + print("ERROR CODES: ", test.result.error_codes) + assert test.result.error_codes[0] == ComputePathToPose.Goal().GOAL_OUTSIDE_MAP test.shutdown() test.info_msg('Done Shutting Down.') From ba0f85bc93c43dc37b4ed69dec5d3430386975ed Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 11:07:51 -0500 Subject: [PATCH 48/61] switched to uint16 --- .../action/ComputePathThroughPoses.action | 24 +++++++++---------- nav2_msgs/action/ComputePathToPose.action | 22 ++++++++--------- nav2_msgs/action/FollowPath.action | 18 +++++++------- nav2_msgs/action/FollowWaypoints.action | 8 +++---- nav2_msgs/action/NavigateThroughPoses.action | 2 +- nav2_msgs/action/NavigateToPose.action | 2 +- 6 files changed, 38 insertions(+), 38 deletions(-) diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index ae1e3db8ed2..38c7f70a5da 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,16 +1,16 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=3000 -int16 INVALID_PLANNER=3001 -int16 TF_ERROR=3002 -int16 START_OUTSIDE_MAP=3003 -int16 GOAL_OUTSIDE_MAP=3004 -int16 START_OCCUPIED=3005 -int16 GOAL_OCCUPIED=3006 -int16 TIMEOUT=3007 -int16 NO_VALID_PATH=3008 -int16 NO_VIAPOINTS_GIVEN=3009 +uint16 NONE=0 +uint16 UNKNOWN=300 +uint16 INVALID_PLANNER=301 +uint16 TF_ERROR=3002 +uint16 START_OUTSIDE_MAP=303 +uint16 GOAL_OUTSIDE_MAP=304 +uint16 START_OCCUPIED=305 +uint16 GOAL_OCCUPIED=306 +uint16 TIMEOUT=3007 +uint16 NO_VALID_PATH=308 +uint16 NO_VIAPOINTS_GIVEN=309 #goal definition geometry_msgs/PoseStamped[] goals @@ -21,6 +21,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time -int16 error_code +uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 93a0171098a..5abf2e6826b 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,15 +1,15 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=2000 -int16 INVALID_PLANNER=2001 -int16 TF_ERROR=2002 -int16 START_OUTSIDE_MAP=2003 -int16 GOAL_OUTSIDE_MAP=2004 -int16 START_OCCUPIED=2005 -int16 GOAL_OCCUPIED=2006 -int16 TIMEOUT=2007 -int16 NO_VALID_PATH=2008 +uint16 NONE=0 +uint16 UNKNOWN=200 +uint16 INVALID_PLANNER=201 +uint16 TF_ERROR=2002 +uint16 START_OUTSIDE_MAP=203 +uint16 GOAL_OUTSIDE_MAP=204 +uint16 START_OCCUPIED=205 +uint16 GOAL_OCCUPIED=206 +uint16 TIMEOUT=207 +uint16 NO_VALID_PATH=208 #goal definition geometry_msgs/PoseStamped goal @@ -20,6 +20,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time -int16 error_code +uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 3b839901d67..c06b918c54a 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,13 +1,13 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1000 -int16 INVALID_CONTROLLER=1001 -int16 TF_ERROR=1002 -int16 INVALID_PATH=1003 -int16 PATIENCE_EXCEEDED=1004 -int16 FAILED_TO_MAKE_PROGRESS=1005 -int16 NO_VALID_CONTROL=1006 +uint16 NONE=0 +uint16 UNKNOWN=100 +uint16 INVALID_CONTROLLER=101 +uint16 TF_ERROR=102 +uint16 INVALID_PATH=103 +uint16 PATIENCE_EXCEEDED=104 +uint16 FAILED_TO_MAKE_PROGRESS=105 +uint16 NO_VALID_CONTROL=106 #goal definition nav_msgs/Path path @@ -16,7 +16,7 @@ string goal_checker_id --- #result definition std_msgs/Empty result -int16 error_code +uint16 error_code --- #feedback definition float32 distance_to_goal diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index 218328cbedd..54763f6d8b7 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,15 +1,15 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=6000 -int16 TASK_EXECUTOR_FAILED=6001 +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 #goal definition geometry_msgs/PoseStamped[] poses --- #result definition int32[] missed_waypoints -int16[] error_codes +uint16[] error_codes --- #feedback definition uint32 current_waypoint diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 33009f45a63..6fee0f0b2ac 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -4,7 +4,7 @@ string behavior_tree --- #result definition std_msgs/Empty result -int16 error_code +uint16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 378eb8e7576..eef0b030c6c 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -4,7 +4,7 @@ string behavior_tree --- #result definition std_msgs/Empty result -int16 error_code +uint16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose From d28d3c5627507f2b785c120818e3f71d7223ac2e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 12:37:08 -0500 Subject: [PATCH 49/61] clean up --- nav2_bringup/params/nav2_params.yaml | 96 ++++++++++--------- .../costmap_filters/costmap_filter.hpp | 1 + .../error_codes/test_error_codes_launch.py | 1 - 3 files changed, 51 insertions(+), 47 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 4577f495507..ae90bc4c61b 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -50,52 +50,56 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + error_code_id_names: + - compute_path_error_code_id + - follow_path_error_code_id + controller_server: ros__parameters: diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 35f53c6b96c..eb1630be263 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "geometry_msgs/msg/pose2_d.hpp" #include "std_srvs/srv/set_bool.hpp" diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index 8594c8c7893..165e1806ce4 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -35,7 +35,6 @@ def generate_launch_description(): load_nodes = GroupAction( actions=[ - # SetParameter("use_sim_time", 'True'), Node( package='tf2_ros', executable='static_transform_publisher', From b0d59f3de10878dcf87b4f68a1085de183c87def Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 17:16:19 -0500 Subject: [PATCH 50/61] code review --- .../src/error_codes/planner/planner_error_plugin.hpp | 2 +- nav2_system_tests/src/waypoint_follower/tester.py | 1 - nav2_waypoint_follower/src/waypoint_follower.cpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index aea388c1655..2b6ae7971d3 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -103,7 +103,7 @@ class NoValidPathErrorPlanner : public UnknownErrorPlanner const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &) override { - throw nav2_core::NoValidPathCouldBeFound("No valid path could be found"); + return nav_msgs::msg::Path(); } }; diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 40ce8e35746..289d50fbce5 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -214,7 +214,6 @@ def main(argv=sys.argv[1:]): result = test.run(True) assert not result result = not result - print("ERROR CODES: ", test.result.error_codes) assert test.result.error_codes[0] == ComputePathToPose.Goal().GOAL_OUTSIDE_MAP test.shutdown() diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 48be6c67ee0..ccda62ccba5 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -245,7 +245,7 @@ WaypointFollower::followWaypoints() is_task_executed ? "succeeded" : "failed!"); if (!is_task_executed) { - missed_waypoints_.emplace_back(MissedWaypoint{goal_index, current_goal_status_.error_code}); + missed_waypoints_.emplace_back(MissedWaypoint{goal_index, ActionT::Goal::TASK_EXECUTOR_FAILED}); } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { From 75b072f90a21d9746660ad2958966167237411b6 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 17:28:20 -0500 Subject: [PATCH 51/61] todo to note --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 6aba308faf1..7754b07ccec 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -277,7 +277,7 @@ void BtActionServer::populateErrorCode( { int highest_priority_error_code = std::numeric_limits::max(); for (const auto & error_code_id : error_code_ids_) { - // TODO(jwallace42): Using try catch due to + // Note: Using try catch due to // /~https://github.com/BehaviorTree/BehaviorTree.CPP/issues/271 try { int current_error_code = blackboard_->get(error_code_id); From 9c16e44d7cae07158e3570b2d50fecdb7fe857dc Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 19:00:29 -0500 Subject: [PATCH 52/61] lint --- nav2_waypoint_follower/src/waypoint_follower.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index ccda62ccba5..cef7e4ad574 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -245,7 +245,9 @@ WaypointFollower::followWaypoints() is_task_executed ? "succeeded" : "failed!"); if (!is_task_executed) { - missed_waypoints_.emplace_back(MissedWaypoint{goal_index, ActionT::Goal::TASK_EXECUTOR_FAILED}); + missed_waypoints_.emplace_back( + MissedWaypoint{goal_index, + ActionT::Goal::TASK_EXECUTOR_FAILED}); } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { From aef83708f9dfd3db845a76c0dc2d4a258714c0a7 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 20:01:47 -0500 Subject: [PATCH 53/61] remove comment --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 7754b07ccec..37cf465eb46 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -277,8 +277,6 @@ void BtActionServer::populateErrorCode( { int highest_priority_error_code = std::numeric_limits::max(); for (const auto & error_code_id : error_code_ids_) { - // Note: Using try catch due to - // /~https://github.com/BehaviorTree/BehaviorTree.CPP/issues/271 try { int current_error_code = blackboard_->get(error_code_id); if (current_error_code != 0 && current_error_code < highest_priority_error_code) { From 4f2d8c9f74711ecfcf1b4bea2d303780aa313a07 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 16 Nov 2022 16:16:23 -0500 Subject: [PATCH 54/61] Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp Co-authored-by: Steve Macenski --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 37cf465eb46..88d19d17530 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -71,7 +71,7 @@ BtActionServer::BtActionServer( for (const auto & error_code_id : error_code_id_names) { error_code_ids_str += error_code_id + "\n"; } - RCLCPP_INFO_STREAM( + RCLCPP_WARN_STREAM( logger_, "Error_code_ids parameter is not set. Using default values of: " << error_code_ids_str << "Make sure these match your BT and there are not other sources of error codes you want " From 3c07fb233f583e6bbe98dab3211958cbcfc6d4df Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 16 Nov 2022 21:56:50 -0500 Subject: [PATCH 55/61] rename error_codes --- .../nav2_behavior_tree/bt_action_server.hpp | 2 +- .../bt_action_server_impl.hpp | 28 +++++++++---------- nav2_bringup/params/nav2_params.yaml | 6 ++-- .../behavior_trees/follow_point.xml | 4 +-- ...replanning_and_if_path_becomes_invalid.xml | 4 +-- ...hrough_poses_w_replanning_and_recovery.xml | 6 ++-- ...gate_to_pose_w_replanning_and_recovery.xml | 4 +-- ..._replanning_goal_patience_and_recovery.xml | 4 +-- ...eplanning_only_if_path_becomes_invalid.xml | 4 +-- .../navigate_w_replanning_distance.xml | 4 +-- ...e_w_replanning_only_if_goal_is_updated.xml | 4 +-- ...eplanning_only_if_path_becomes_invalid.xml | 4 +-- .../navigate_w_replanning_speed.xml | 4 +-- .../navigate_w_replanning_time.xml | 4 +-- 14 files changed, 41 insertions(+), 41 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index e8a6778f1c4..79b1df5bb40 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -219,7 +219,7 @@ class BtActionServer std::vector plugin_lib_names_; // Error code id names - std::vector error_code_ids_; + std::vector error_code_names_; // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 88d19d17530..c3f9244f173 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -61,22 +61,22 @@ BtActionServer::BtActionServer( node->declare_parameter("default_server_timeout", 20); } - std::vector error_code_id_names = { - "follow_path_error_code_id", - "compute_path_error_code_id" + std::vector error_code_names = { + "follow_path_error_code", + "compute_path_error_code" }; - if (!node->has_parameter("error_code_id_names")) { - std::string error_code_ids_str; - for (const auto & error_code_id : error_code_id_names) { - error_code_ids_str += error_code_id + "\n"; + if (!node->has_parameter("error_code_names")) { + std::string error_codes_str; + for (const auto & error_code : error_code_names) { + error_codes_str += error_code + "\n"; } RCLCPP_WARN_STREAM( - logger_, "Error_code_ids parameter is not set. Using default values of: " - << error_code_ids_str + logger_, "Error_code parameters were not set. Using default values of: " + << error_codes_str << "Make sure these match your BT and there are not other sources of error codes you want " "reported to your application"); - node->declare_parameter("error_code_id_names", error_code_id_names); + node->declare_parameter("error_code_names", error_code_names); } } @@ -124,7 +124,7 @@ bool BtActionServer::on_configure() default_server_timeout_ = std::chrono::milliseconds(timeout); // Get error code id names to grab off of the blackboard - error_code_ids_ = node->get_parameter("error_code_id_names").as_string_array(); + error_code_names_ = node->get_parameter("error_code_names").as_string_array(); // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -276,9 +276,9 @@ void BtActionServer::populateErrorCode( typename std::shared_ptr result) { int highest_priority_error_code = std::numeric_limits::max(); - for (const auto & error_code_id : error_code_ids_) { + for (const auto & error_code : error_code_names_) { try { - int current_error_code = blackboard_->get(error_code_id); + int current_error_code = blackboard_->get(error_code); if (current_error_code != 0 && current_error_code < highest_priority_error_code) { highest_priority_error_code = current_error_code; } @@ -286,7 +286,7 @@ void BtActionServer::populateErrorCode( RCLCPP_ERROR( logger_, "Failed to get error code: %s from blackboard", - error_code_id.c_str()); + error_code.c_str()); } } diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index ae90bc4c61b..69a1f2507e3 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -96,9 +96,9 @@ bt_navigator: - nav2_back_up_cancel_bt_node - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node - error_code_id_names: - - compute_path_error_code_id - - follow_path_error_code_id + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index c61d59a5199..930a2691fb6 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -8,13 +8,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index fb79bdd70c4..0a00a60a4f5 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -19,13 +19,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 9b881e70bb4..3dc5a765031 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -11,13 +11,13 @@ - + - + ` - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 412a4b7bfa6..d50bd7fe5df 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -10,12 +10,12 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 8af39bbe2be..e2bbeb12c73 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -11,7 +11,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index b16809e7e53..bd4e1c9aca8 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -17,13 +17,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index b9db08c8c48..9345d8f66d6 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 1fd2b65fd10..4ba2f67cc97 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index 4e8df5d2941..753de5d7802 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -12,10 +12,10 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index b85d39b682c..97b70b7a62d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 6df23df699d..db0e733db1a 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -6,9 +6,9 @@ - + - + From 2f314273e728da61597189ecae3bb961d961b546 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 17 Nov 2022 12:18:43 -0500 Subject: [PATCH 56/61] error code for navigate to pose --- .../action/navigate_to_pose_action.hpp | 21 ++++++++++++++++ .../action/navigate_to_pose_action.cpp | 24 ++++++++++++++++--- nav2_msgs/action/NavigateToPose.action | 4 ++++ 3 files changed, 46 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index 3708017f16a..59ed2a7d7e1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class NavigateToPoseAction : public BtActionNode { + using Action = nav2_msgs::action::NavigateToPose; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::NavigateToPoseAction @@ -47,6 +51,21 @@ class NavigateToPoseAction : public BtActionNode("goal", "Destination to plan to"), BT::InputPort("behavior_tree", "Behavior tree to run"), + BT::OutputPort( + "navigate_to_pose_error_code", "Navigate to pose error code"), }); } }; 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 82cdab44f84..7f334f9d7da 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -24,9 +24,8 @@ NavigateToPoseAction::NavigateToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) -{ -} +: BtActionNode(xml_tag_name, action_name, conf) +{} void NavigateToPoseAction::on_tick() { @@ -39,6 +38,25 @@ void NavigateToPoseAction::on_tick() getInput("behavior_tree", goal_.behavior_tree); } +BT::NodeStatus NavigateToPoseAction::on_success() +{ + setOutput("navigate_to_pose_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateToPoseAction::on_aborted() +{ + setOutput("navigate_to_pose_error_code", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus NavigateToPoseAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("navigate_to_pose_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index eef0b030c6c..b38aa0002cb 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -1,3 +1,7 @@ +# 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 From 7b098e4195db511b32588ec584fe6f066b375e78 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 17 Nov 2022 12:39:42 -0500 Subject: [PATCH 57/61] error codes for navigate through poses. --- nav2_msgs/action/NavigateThroughPoses.action | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 6fee0f0b2ac..ed1f8ca5f1f 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,3 +1,7 @@ +# 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 From c9dcc79715657941fa1d4abf359ed68a3cc4f8f3 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 17 Nov 2022 12:41:21 -0500 Subject: [PATCH 58/61] error codes for navigate through poses --- .../action/navigate_through_poses_action.hpp | 21 +++++++++++++++++++ .../action/navigate_through_poses_action.cpp | 20 ++++++++++++++++++ 2 files changed, 41 insertions(+) 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 7e7f3d5c30b..bfcdc320d85 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 @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class NavigateThroughPosesAction : public BtActionNode { + using Action = nav2_msgs::action::NavigateThroughPoses; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction @@ -47,6 +51,21 @@ class NavigateThroughPosesAction : public BtActionNode("goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), + BT::OutputPort( + "navigate_through_poses_error_code", "The navigate through poses error code"), }); } }; 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 693fbfa146c..9cfc7f830e5 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -39,6 +39,26 @@ void NavigateThroughPosesAction::on_tick() getInput("behavior_tree", goal_.behavior_tree); } +BT::NodeStatus NavigateThroughPosesAction::on_success() +{ + setOutput("navigate_through_poses_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateThroughPosesAction::on_aborted() +{ + setOutput("navigate_through_poses_error_code", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus NavigateThroughPosesAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("navigate_through_poses_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" From fb7b98cf58acca3051af093b9364072873493f52 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 17 Nov 2022 17:12:54 -0500 Subject: [PATCH 59/61] message update for waypoint follower --- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/action/FollowWaypoints.action | 3 +- nav2_msgs/msg/MissedWaypoint.msg | 3 ++ .../src/waypoint_follower/tester.py | 3 +- .../waypoint_follower.hpp | 8 +---- .../src/waypoint_follower.cpp | 34 ++++++------------- 6 files changed, 18 insertions(+), 34 deletions(-) create mode 100644 nav2_msgs/msg/MissedWaypoint.msg diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2be81db0a74..09fbc082268 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/MissedWaypoint.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index 54763f6d8b7..12f9a39760b 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -8,8 +8,7 @@ uint16 TASK_EXECUTOR_FAILED=601 geometry_msgs/PoseStamped[] poses --- #result definition -int32[] missed_waypoints -uint16[] error_codes +MissedWaypoint[] missed_waypoints --- #feedback definition uint32 current_waypoint diff --git a/nav2_msgs/msg/MissedWaypoint.msg b/nav2_msgs/msg/MissedWaypoint.msg new file mode 100644 index 00000000000..44b2dc3b9cc --- /dev/null +++ b/nav2_msgs/msg/MissedWaypoint.msg @@ -0,0 +1,3 @@ +uint32 index +geometry_msgs/PoseStamped goal +uint16 error_code diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 9a3a8fa720b..2e88a63a7a7 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -230,7 +230,8 @@ def main(argv=sys.argv[1:]): result = test.run(True, False) assert not result result = not result - assert test.action_result.error_codes[0] == ComputePathToPose.Goal().GOAL_OUTSIDE_MAP + assert test.action_result.missed_waypoints[0].error_code == \ + ComputePathToPose.Goal().GOAL_OUTSIDE_MAP # stop on failure test with bogous waypoint test.setStopFailureParam(True) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 2164bea55a0..e5c59aa20b6 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -23,6 +23,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" +#include "nav2_msgs/msg/missed_waypoint.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -43,12 +44,6 @@ enum class ActionStatus SUCCEEDED = 3 }; -struct MissedWaypoint -{ - unsigned int index; - int error_code; -}; - struct GoalStatus { ActionStatus status; @@ -149,7 +144,6 @@ class WaypointFollower : public nav2_util::LifecycleNode bool stop_on_failure_; int loop_rate_; GoalStatus current_goal_status_; - std::vector missed_waypoints_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index cef7e4ad574..2bb95fea7dd 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -212,21 +212,18 @@ WaypointFollower::followWaypoints() action_server_->publish_feedback(feedback); if (current_goal_status_.status == ActionStatus::FAILED) { - missed_waypoints_.emplace_back(MissedWaypoint{goal_index, current_goal_status_.error_code}); + nav2_msgs::msg::MissedWaypoint missedWaypoint; + missedWaypoint.index = goal_index; + missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.error_code = current_goal_status_.error_code; + result->missed_waypoints.push_back(missedWaypoint); if (stop_on_failure_) { RCLCPP_WARN( get_logger(), "Failed to process waypoint %i in waypoint " "list and stop on failure is enabled." " Terminating action.", goal_index); - - for (const auto missed_waypoint : missed_waypoints_) { - result->missed_waypoints.push_back(missed_waypoint.index); - result->error_codes.push_back(missed_waypoint.error_code); - } - action_server_->terminate_current(result); - missed_waypoints_.clear(); current_goal_status_.error_code = 0; return; } else { @@ -245,9 +242,11 @@ WaypointFollower::followWaypoints() is_task_executed ? "succeeded" : "failed!"); if (!is_task_executed) { - missed_waypoints_.emplace_back( - MissedWaypoint{goal_index, - ActionT::Goal::TASK_EXECUTOR_FAILED}); + 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; + result->missed_waypoints.push_back(missedWaypoint); } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { @@ -256,13 +255,7 @@ WaypointFollower::followWaypoints() " stop on failure is enabled." " Terminating action.", goal_index); - for (const auto missed_waypoint : missed_waypoints_) { - result->missed_waypoints.push_back(missed_waypoint.index); - result->error_codes.push_back(missed_waypoint.error_code); - } - action_server_->terminate_current(result); - missed_waypoints_.clear(); current_goal_status_.error_code = 0; return; } else { @@ -282,14 +275,7 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", goal->poses.size()); - - for (const auto missed_waypoint : missed_waypoints_) { - result->missed_waypoints.push_back(missed_waypoint.index); - result->error_codes.push_back(missed_waypoint.error_code); - } - action_server_->succeeded_current(result); - missed_waypoints_.clear(); current_goal_status_.error_code = 0; return; } From 4d6a78dca8efd916813729a396843f03b5ba5565 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 17 Nov 2022 17:19:14 -0500 Subject: [PATCH 60/61] rename to error code --- .../plugins/action/navigate_through_poses_action.hpp | 2 +- .../plugins/action/navigate_to_pose_action.hpp | 2 +- .../plugins/action/navigate_through_poses_action.cpp | 6 +++--- .../plugins/action/navigate_to_pose_action.cpp | 6 +++--- 4 files changed, 8 insertions(+), 8 deletions(-) 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 bfcdc320d85..45d0adff108 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 @@ -77,7 +77,7 @@ class NavigateThroughPosesAction : public BtActionNode("goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( - "navigate_through_poses_error_code", "The navigate through poses error code"), + "error_code_id", "The navigate through poses error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index 59ed2a7d7e1..51c59743904 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -77,7 +77,7 @@ class NavigateToPoseAction : public BtActionNode("goal", "Destination to plan to"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( - "navigate_to_pose_error_code", "Navigate to pose error code"), + "error_code_id", "Navigate to pose error code"), }); } }; 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 9cfc7f830e5..5e3e65f424b 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -41,20 +41,20 @@ void NavigateThroughPosesAction::on_tick() BT::NodeStatus NavigateThroughPosesAction::on_success() { - setOutput("navigate_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } BT::NodeStatus NavigateThroughPosesAction::on_aborted() { - setOutput("navigate_through_poses_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } BT::NodeStatus NavigateThroughPosesAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("navigate_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::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 7f334f9d7da..dabc576fccd 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -40,20 +40,20 @@ void NavigateToPoseAction::on_tick() BT::NodeStatus NavigateToPoseAction::on_success() { - setOutput("navigate_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } BT::NodeStatus NavigateToPoseAction::on_aborted() { - setOutput("navigate_to_pose_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } BT::NodeStatus NavigateToPoseAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("navigate_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } From ff14a74d5d17db93190ac1d6f0df913ac5f5663b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 17 Nov 2022 17:24:13 -0500 Subject: [PATCH 61/61] update node xml --- nav2_behavior_tree/nav2_tree_nodes.xml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 95e5a0917da..a6eb71f3de6 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,7 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node - "The compute path to pose error code" + "Compute path to pose error code" @@ -87,8 +87,7 @@ Server timeout Mapped name to the planner plugin type to use Path created by ComputePathToPose node - "The compute path through poses - pose error code" + "Compute path through poses error code" @@ -115,7 +114,7 @@ Goal checker Service name Server timeout - Follow Path error code + Follow Path error code @@ -123,6 +122,7 @@ Service name Server timeout Behavior tree to run + Navigate to pose error code @@ -130,6 +130,7 @@ Service name Server timeout Behavior tree to run + Navigate through poses error code