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..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 @@ -189,6 +189,13 @@ 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 std::string action_name_; @@ -211,6 +218,9 @@ class BtActionServer // Libraries to pull plugins (BT Nodes) from std::vector plugin_lib_names_; + // Error code id names + 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 592d1257a27..96c191f724f 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" @@ -59,6 +60,24 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + + std::vector error_code_names = { + "follow_path_error_code", + "compute_path_error_code" + }; + + 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 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_names", error_code_names); + } } template @@ -101,6 +120,9 @@ 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_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_); @@ -223,6 +245,9 @@ 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(); + + populateErrorCode(result); + on_completion_callback_(result, rc); switch (rc) { @@ -243,6 +268,30 @@ void BtActionServer::executeCallback() } } +template +void BtActionServer::populateErrorCode( + typename std::shared_ptr result) +{ + int highest_priority_error_code = std::numeric_limits::max(); + for (const auto & error_code : error_code_names_) { + try { + 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; + } + } catch (...) { + RCLCPP_ERROR( + logger_, + "Failed to get error code: %s from blackboard", + error_code.c_str()); + } + } + + if (highest_priority_error_code != std::numeric_limits::max()) { + result->error_code = highest_priority_error_code; + } +} + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ 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 1c7f0526dfd..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 @@ -86,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 369e7ab0f98..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( - "follow_path_error_code", "The follow path error code"), + "error_code_id", "The follow path error code"), }); } }; 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..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 @@ -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( + "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 3708017f16a..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 @@ -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( + "error_code_id", "Navigate to pose error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 2f719305d73..90c56e91c1d 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,7 +77,7 @@ Server 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 Server name Server timeout - Follow Path error code + Follow Path error code @@ -123,6 +122,7 @@ Server name Server timeout Behavior tree to run + Navigate to pose error code @@ -130,6 +130,7 @@ Server name Server timeout Behavior tree to run + Navigate through poses 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 68a909b52c7..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); - setOutput("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 c90d8fb65a1..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() { - setOutput("follow_path_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } BT::NodeStatus FollowPathAction::on_aborted() { - setOutput("follow_path_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 - 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/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 693fbfa146c..5e3e65f424b 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("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateThroughPosesAction::on_aborted() +{ + 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("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" 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..dabc576fccd 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("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateToPoseAction::on_aborted() +{ + 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("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index e3ba0a665fb..7601e039a4a 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -104,6 +104,10 @@ bt_navigator: - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ffae8d9b2ab..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 f4ed518444d..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 1378863cdf6..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 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/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..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 @@ -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..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 8ca5dbbc5ac..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 3e889440a88..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 e921db55578..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 @@ -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 f0f47c496a0..97b70b7a62d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -5,10 +5,10 @@ - - + + - + 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..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 @@ - + - + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9c4ae92d36c..2e876b2e76e 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 a8303f79d47..cbd686cc040 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" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 154efef2bee..a0bd0cec146 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -371,7 +371,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; @@ -421,6 +421,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_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 49aeab21e38..c336d3c0911 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 @@ -43,6 +43,7 @@ #include #include +#include #include "geometry_msgs/msg/pose2_d.hpp" #include "std_srvs/srv/set_bool.hpp" diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index fe8e0c4e107..82c618689d8 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/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index c37297c69e7..38c7f70a5da 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,15 +1,16 @@ # 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 +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 @@ -20,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 7e727088a15..5abf2e6826b 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,14 +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 +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 @@ -19,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 9ab82ff4d76..c06b918c54a 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,12 +1,13 @@ # 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 +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 @@ -15,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 c4b47648219..12f9a39760b 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,8 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 + #goal definition geometry_msgs/PoseStamped[] poses --- #result definition -int32[] missed_waypoints +MissedWaypoint[] missed_waypoints --- #feedback definition uint32 current_waypoint diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index e2a57f25a94..ed1f8ca5f1f 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,9 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + #goal definition geometry_msgs/PoseStamped[] poses string behavior_tree --- #result definition std_msgs/Empty result +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 b7077922418..b38aa0002cb 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -1,9 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + #goal definition geometry_msgs/PoseStamped pose string behavior_tree --- #result definition std_msgs/Empty result +uint16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose 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_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 9094a64ce03..813dff63a5e 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -409,7 +409,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 @@ -433,6 +433,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; @@ -508,7 +512,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 @@ -524,6 +528,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; @@ -584,6 +592,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_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 3421c3f000d..439dab22dcc 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -345,22 +345,28 @@ 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) + 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...") @@ -383,11 +389,21 @@ def getPathThroughPoses(self, start, goals, 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 + + 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 6d56df1ef7e..558ae809bd4 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,8 +101,11 @@ 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() +ament_export_libraries(${local_controller_plugin_lib}) +ament_export_libraries(${global_planner_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index bfdee60c710..c9482808357 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..efcdac9c82b --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp @@ -0,0 +1,25 @@ +// 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..ba194b195d8 --- /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..4c3e8395204 --- /dev/null +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -0,0 +1,175 @@ +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.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"] + 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: + 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: ["obstacle_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 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + 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" 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..2b6ae7971d3 --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -0,0 +1,144 @@ +// 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 + { + return nav_msgs::msg::Path(); + } +}; + + +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 new file mode 100755 index 00000000000..3079e3a3fc1 --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -0,0 +1,130 @@ +#! /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 + +import rclpy +from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses + + +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 + + # 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, + 'invalid_controller': FollowPath.Goal().INVALID_CONTROLLER, + 'tf_error': FollowPath.Goal().TF_ERROR, + 'invalid_path': FollowPath.Goal().INVALID_PATH, + 'patience_exceeded': FollowPath.Goal().PATIENCE_EXCEEDED, + 'failed_to_make_progress': FollowPath.Goal().FAILED_TO_MAKE_PROGRESS, + 'no_valid_control': FollowPath.Goal().NO_VALID_CONTROL + } + + 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" + + 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, + 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, + 'tf_error': ComputePathToPose.Goal().TF_ERROR, + 'start_outside_map': ComputePathToPose.Goal().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathToPose.Goal().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathToPose.Goal().START_OCCUPIED, + 'goal_occupied': ComputePathToPose.Goal().GOAL_OCCUPIED, + 'timeout': ComputePathToPose.Goal().TIMEOUT, + 'no_valid_path': ComputePathToPose.Goal().NO_VALID_PATH} + + 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, + 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, + 'tf_error': ComputePathThroughPoses.Goal().TF_ERROR, + 'start_outside_map': ComputePathThroughPoses.Goal().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathThroughPoses.Goal().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathThroughPoses.Goal().START_OCCUPIED, + 'goal_occupied': ComputePathThroughPoses.Goal().GOAL_OCCUPIED, + 'timeout': ComputePathThroughPoses.Goal().TIMEOUT, + 'no_valid_path': ComputePathThroughPoses.Goal().NO_VALID_PATH, + 'no_viapoints_given': ComputePathThroughPoses.Goal().NO_VIAPOINTS_GIVEN} + + 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() + 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..165e1806ce4 --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -0,0 +1,94 @@ +#! /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 launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +from launch_testing.legacy import LaunchTestService +from launch_ros.actions import Node +from launch.actions import GroupAction + + +def generate_launch_description(): + params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') + + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + lifecycle_nodes = ['controller_server', + 'planner_server'] + + load_nodes = GroupAction( + actions=[ + 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:]): + 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()) 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); } diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index e6d299caa55..420c5df6e64 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 @@ -110,9 +110,9 @@ def run(self, block): 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.action_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.action_result.missed_waypoints))) return False self.info_msg('Goal succeeded!') @@ -208,12 +208,14 @@ 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 test.action_result.missed_waypoints[0].error_code == \ + ComputePathToPose.Goal().GOAL_OUTSIDE_MAP test.shutdown() test.info_msg('Done Shutting Down.') 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..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,6 +44,12 @@ enum class ActionStatus SUCCEEDED = 3 }; +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 @@ -133,10 +140,10 @@ class WaypointFollower : public nav2_util::LifecycleNode rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; + bool stop_on_failure_; - ActionStatus current_goal_status_; int loop_rate_; - std::vector failed_ids_; + GoalStatus current_goal_status_; // 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 a04c404c772..2bb95fea7dd 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -205,30 +205,33 @@ 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) { - failed_ids_.push_back(goal_index); + if (current_goal_status_.status == ActionStatus::FAILED) { + 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); - result->missed_waypoints = failed_ids_; action_server_->terminate_current(result); - failed_ids_.clear(); + 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); @@ -237,16 +240,23 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); + + if (!is_task_executed) { + 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_) { - 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_; + action_server_->terminate_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } else { RCLCPP_INFO( @@ -255,8 +265,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++; @@ -265,9 +275,8 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", goal->poses.size()); - result->missed_waypoints = failed_ids_; action_server_->succeeded_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } } else { @@ -296,16 +305,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; + 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; } } @@ -318,7 +328,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; } }