diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp index 982f4161ea4..4bf6dbb9344 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class SmoothPathAction : public nav2_behavior_tree::BtActionNode { + using Action = nav2_msgs::action::SmoothPath; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::SmoothPathAction @@ -52,6 +56,16 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode( - "smoothed_path", - "Path smoothed by SmootherServer node"), - BT::OutputPort("smoothing_duration", "Time taken to smooth path"), - BT::OutputPort( - "was_completed", "True if smoothing was not interrupted by time limit"), BT::InputPort("unsmoothed_path", "Path to be smoothed"), BT::InputPort("max_smoothing_duration", 3.0, "Maximum smoothing duration"), BT::InputPort( "check_for_collisions", false, "If true collision check will be performed after smoothing"), BT::InputPort("smoother_id", ""), + BT::OutputPort( + "smoothed_path", + "Path smoothed by SmootherServer node"), + BT::OutputPort("smoothing_duration", "Time taken to smooth path"), + BT::OutputPort( + "was_completed", "True if smoothing was not interrupted by time limit"), + BT::OutputPort( + "error_code_id", "The smooth path error code"), }); } }; diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index bfbc841ff1b..b3291b73ccc 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -44,6 +44,21 @@ BT::NodeStatus SmoothPathAction::on_success() setOutput("smoothed_path", result_.result->path); setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds()); setOutput("was_completed", result_.result->was_completed); + // Set empty error code, action was successful + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus SmoothPathAction::on_aborted() +{ + setOutput("error_code_id", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus SmoothPathAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp index b5432bf6591..aa3ba1eb024 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp @@ -28,6 +28,7 @@ #include "nav2_constrained_smoother/smoother_cost_function.hpp" #include "nav2_constrained_smoother/utils.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "ceres/ceres.h" #include "Eigen/Core" @@ -94,7 +95,7 @@ class Smoother { // Path has always at least 2 points if (path.size() < 2) { - throw std::runtime_error("Constrained smoother: Path must have at least 2 points"); + throw nav2_core::InvalidPath("Constrained smoother: Path must have at least 2 points"); } options_.max_solver_time_in_seconds = params.max_time; @@ -110,7 +111,7 @@ class Smoother RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "%s", summary.FullReport().c_str()); } if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost < 0.0) { - return false; + throw nav2_core::FailedToSmoothPath("Solution is not usable"); } } else { RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "Path too short to optimize"); diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 4ce698cda16..305f4141f3c 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -23,7 +23,7 @@ #include "nav2_constrained_smoother/constrained_smoother.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" @@ -137,8 +137,8 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat logger_, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", plugin_name_.c_str()); - throw std::runtime_error( - "Failed to smooth plan, Ceres could not find a usable solution."); + throw nav2_core::FailedToSmoothPath( + "Failed to smooth plan, Ceres could not find a usable solution"); } // populate final path diff --git a/nav2_core/include/nav2_core/smoother_exceptions.hpp b/nav2_core/include/nav2_core/smoother_exceptions.hpp new file mode 100644 index 00000000000..fcd38dc3b47 --- /dev/null +++ b/nav2_core/include/nav2_core/smoother_exceptions.hpp @@ -0,0 +1,67 @@ +// 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 NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_ +#define NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_ + +#include +#include + +namespace nav2_core +{ + +class SmootherException : public std::runtime_error +{ +public: + explicit SmootherException(const std::string & description) + : std::runtime_error(description) {} +}; + +class InvalidSmoother : public SmootherException +{ +public: + explicit InvalidSmoother(const std::string & description) + : SmootherException(description) {} +}; + +class InvalidPath : public SmootherException +{ +public: + explicit InvalidPath(const std::string & description) + : SmootherException(description) {} +}; + +class SmootherTimedOut : public SmootherException +{ +public: + explicit SmootherTimedOut(const std::string & description) + : SmootherException(description) {} +}; + +class SmoothedPathInCollision : public SmootherException +{ +public: + explicit SmoothedPathInCollision(const std::string & description) + : SmootherException(description) {} +}; + +class FailedToSmoothPath : public SmootherException +{ +public: + explicit FailedToSmoothPath(const std::string & description) + : SmootherException(description) {} +}; + +} // namespace nav2_core +#endif // NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/SmoothPath.action b/nav2_msgs/action/SmoothPath.action index 18560d70b97..d7443fce5aa 100644 --- a/nav2_msgs/action/SmoothPath.action +++ b/nav2_msgs/action/SmoothPath.action @@ -1,3 +1,13 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=500 +uint16 INVALID_SMOOTHER=501 +uint16 TIMEOUT=502 +uint16 SMOOTHED_PATH_IN_COLLISION=503 +uint16 FAILED_TO_SMOOTH_PATH=504 +uint16 INVALID_PATH=505 + #goal definition nav_msgs/Path path string smoother_id @@ -8,5 +18,6 @@ bool check_for_collisions nav_msgs/Path path builtin_interfaces/Duration smoothing_duration bool was_completed +uint16 error_code --- #feedback definition diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 5b00c6a49ea..0ce475f63f6 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -433,16 +433,18 @@ def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_coll 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 smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): """Send a `SmoothPath` action request.""" rtn = self._smoothPathImpl( - self, path, smoother_id='', max_duration=2.0, check_for_collision=False) + path, smoother_id='', max_duration=2.0, check_for_collision=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: diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index 1a4d4165037..b52ebf65219 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -114,6 +114,7 @@ class SmootherServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::SmoothPath; + using ActionGoal = Action::Goal; using ActionServer = nav2_util::SimpleActionServer; /** @@ -135,6 +136,13 @@ class SmootherServer : public nav2_util::LifecycleNode */ bool findSmootherId(const std::string & c_name, std::string & name); + /** + * @brief Validate that the path contains a meaningful path for smoothing + * @param path current path + * return bool if the path is valid + */ + bool validate(const nav_msgs::msg::Path & path); + // Our action server implements the SmoothPath action std::unique_ptr action_server_; diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index f8f48c9886a..4e3588436a7 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -20,7 +20,7 @@ #include #include -#include "nav2_core/planner_exceptions.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "nav2_smoother/nav2_smoother.hpp" #include "nav2_util/node_utils.hpp" #include "nav_2d_utils/conversions.hpp" @@ -263,13 +263,17 @@ void SmootherServer::smoothPlan() if (findSmootherId(c_name, current_smoother)) { current_smoother_ = current_smoother; } else { - action_server_->terminate_current(); - return; + throw nav2_core::InvalidSmoother("Invalid Smoother: " + c_name); } // Perform smoothing auto goal = action_server_->get_current_goal(); result->path = goal->path; + + if (!validate(result->path)) { + throw nav2_core::InvalidPath("Requested path to smooth is invalid"); + } + result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); result->smoothing_duration = steady_clock_.now() - start_time; @@ -283,6 +287,7 @@ void SmootherServer::smoothPlan() rclcpp::Duration(goal->max_smoothing_duration).seconds(), rclcpp::Duration(result->smoothing_duration).seconds()); } + plan_publisher_->publish(result->path); // Check for collisions @@ -299,8 +304,11 @@ void SmootherServer::smoothPlan() get_logger(), "Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf", pose2d.x, pose2d.y, pose2d.theta); - action_server_->terminate_current(result); - return; + throw nav2_core::SmoothedPathInCollision( + "Smoothed Path collided at" + "X: " + std::to_string(pose2d.x) + + "Y: " + std::to_string(pose2d.y) + + "Theta: " + std::to_string(pose2d.theta)); } fetch_data = false; } @@ -311,13 +319,50 @@ void SmootherServer::smoothPlan() rclcpp::Duration(result->smoothing_duration).seconds()); action_server_->succeeded_current(result); - } catch (nav2_core::PlannerException & e) { - RCLCPP_ERROR(this->get_logger(), e.what()); - action_server_->terminate_current(); + } catch (nav2_core::InvalidSmoother & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::INVALID_SMOOTHER; + action_server_->terminate_current(result); + return; + } catch (nav2_core::SmootherTimedOut & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::TIMEOUT; + action_server_->terminate_current(result); + return; + } catch (nav2_core::SmoothedPathInCollision & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::SMOOTHED_PATH_IN_COLLISION; + action_server_->terminate_current(result); + return; + } catch (nav2_core::FailedToSmoothPath & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::FAILED_TO_SMOOTH_PATH; + action_server_->terminate_current(result); + return; + } catch (nav2_core::InvalidPath & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::INVALID_PATH; + action_server_->terminate_current(result); + return; + } catch (nav2_core::SmootherException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + result->error_code = ActionGoal::UNKNOWN; + action_server_->terminate_current(result); return; } } +bool SmootherServer::validate(const nav_msgs::msg::Path & path) +{ + if (path.poses.empty()) { + RCLCPP_WARN(get_logger(), "Requested path to smooth is empty"); + return false; + } + + RCLCPP_DEBUG(get_logger(), "Requested path to smooth is valid"); + return true; +} + } // namespace nav2_smoother #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 6a61196ab32..17fe63a926b 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -15,6 +15,7 @@ #include #include #include "nav2_smoother/savitzky_golay_smoother.hpp" +#include "nav2_core/smoother_exceptions.hpp" namespace nav2_smoother { @@ -71,7 +72,7 @@ bool SavitzkyGolaySmoother::smooth( RCLCPP_WARN( logger_, "Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds()); - return false; + throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration"); } // Smooth path segment diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 2db6ce3c927..29f3b89a828 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -15,6 +15,7 @@ #include #include #include "nav2_smoother/simple_smoother.hpp" +#include "nav2_core/smoother_exceptions.hpp" namespace nav2_smoother { @@ -65,6 +66,7 @@ bool SimpleSmoother::smooth( double time_remaining = max_time.seconds(); bool success = true, reversing_segment; + unsigned int segments_smoothed = 0; nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; @@ -84,10 +86,16 @@ bool SimpleSmoother::smooth( time_remaining = max_time.seconds() - duration_cast>(now - start).count(); refinement_ctr_ = 0; - // Smooth path segment naively - success = success && smoothImpl( + bool segment_was_smoothed = smoothImpl( curr_path_segment, reversing_segment, costmap.get(), time_remaining); + if (segment_was_smoothed) { + segments_smoothed++; + } + + // Smooth path segment naively + success = success && segment_was_smoothed; + // Assemble the path changes to the main path std::copy( curr_path_segment.poses.begin(), @@ -96,6 +104,10 @@ bool SimpleSmoother::smooth( } } + if (segments_smoothed == 0) { + throw nav2_core::FailedToSmoothPath("No segments were smoothed"); + } + return success; } @@ -140,7 +152,7 @@ bool SimpleSmoother::smoothImpl( "Smoothing time exceeded allowed duration of %0.2f.", max_time); path = last_path; updateApproximatePathOrientations(path, reversing_segment); - return false; + throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration"); } for (unsigned int i = 1; i != path_size - 1; i++) { diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp index 58860d67db9..89df4c3a4fa 100644 --- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -22,6 +22,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "nav2_core/smoother_exceptions.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_msgs/msg/costmap.hpp" @@ -109,7 +110,7 @@ TEST(SmootherTest, test_sg_smoother_basics) // Attempt smoothing with no time given, should fail rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds - EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time)); + EXPECT_THROW(smoother->smooth(straight_regular_path, no_time), nav2_core::SmootherTimedOut); smoother->deactivate(); smoother->cleanup(); diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index fc3dceec971..c8f7da8e871 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -14,19 +14,15 @@ #include #include -#include #include -#include -#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_smoother/simple_smoother.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "nav2_core/smoother_exceptions.hpp" using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT @@ -121,7 +117,7 @@ TEST(SmootherTest, test_simple_smoother) rclcpp::Duration no_time = rclcpp::Duration::from_seconds(0.0); // 0 seconds rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1); // 1 second - EXPECT_FALSE(smoother->smooth(straight_irregular_path, no_time)); + EXPECT_THROW(smoother->smooth(straight_irregular_path, no_time), nav2_core::SmootherTimedOut); EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time)); for (uint i = 0; i != straight_irregular_path.poses.size() - 1; i++) { // Check distances are more evenly spaced out now @@ -209,7 +205,7 @@ TEST(SmootherTest, test_simple_smoother) collision_path.poses[9].pose.position.y = 1.4; collision_path.poses[10].pose.position.x = 1.5; collision_path.poses[10].pose.position.y = 1.5; - EXPECT_FALSE(smoother->smooth(collision_path, max_time)); + EXPECT_THROW(smoother->smooth(collision_path, max_time), nav2_core::FailedToSmoothPath); // test cusp / reversing segments nav_msgs::msg::Path reversing_path; @@ -236,9 +232,9 @@ TEST(SmootherTest, test_simple_smoother) reversing_path.poses[9].pose.position.y = 0.1; reversing_path.poses[10].pose.position.x = 0.5; reversing_path.poses[10].pose.position.y = 0.0; - EXPECT_TRUE(smoother->smooth(reversing_path, max_time)); + EXPECT_THROW(smoother->smooth(reversing_path, max_time), nav2_core::FailedToSmoothPath); - // // test rotate in place + // test rotate in place tf2::Quaternion quat1, quat2; quat1.setRPY(0.0, 0.0, 0.0); quat2.setRPY(0.0, 0.0, 1.0); @@ -248,7 +244,7 @@ TEST(SmootherTest, test_simple_smoother) straight_irregular_path.poses[6].pose.position.x = 0.5; straight_irregular_path.poses[6].pose.position.y = 0.5; straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(quat2); - EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time)); + EXPECT_THROW(smoother->smooth(straight_irregular_path, max_time), nav2_core::FailedToSmoothPath); // test max iterations smoother->setMaxItsToInvalid(); @@ -276,5 +272,5 @@ TEST(SmootherTest, test_simple_smoother) max_its_path.poses[9].pose.position.y = 0.9; max_its_path.poses[10].pose.position.x = 0.5; max_its_path.poses[10].pose.position.y = 1.0; - EXPECT_FALSE(smoother->smooth(max_its_path, max_time)); + EXPECT_THROW(smoother->smooth(max_its_path, max_time), nav2_core::FailedToSmoothPath); } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 558ae809bd4..e65b2f928d0 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -81,6 +81,23 @@ install(FILES src/error_codes/planner_plugins.xml DESTINATION share/${PROJECT_NAME} ) +set(smoother_plugin_lib smoother) + +add_library(${smoother_plugin_lib} SHARED src/error_codes/smoother/smoother_error_plugin.cpp) +ament_target_dependencies(${smoother_plugin_lib} ${dependencies}) +target_compile_definitions(${smoother_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/smoother_plugins.xml) + +install(TARGETS ${smoother_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/smoother_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -108,4 +125,5 @@ endif() ament_export_libraries(${local_controller_plugin_lib}) ament_export_libraries(${global_planner_plugin_lib}) +ament_export_libraries(${smoother_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 12f269a2d9a..32d05a49f89 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -67,5 +67,6 @@ ament_cmake + 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 4c3e8395204..388e6f2cdb2 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -173,3 +173,18 @@ planner_server: plugin: "nav2_system_tests::NoValidPathErrorPlanner" no_viapoints_given: plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" + +smoother_server: + ros__parameters: + smoother_plugins: ["unknown", "timeout", "smoothed_path_in_collision", + "failed_to_smooth_path", "invalid_path"] + unknown: + plugin: "nav2_system_tests::UnknownErrorSmoother" + timeout: + plugin: "nav2_system_tests::TimeOutErrorSmoother" + smoothed_path_in_collision: + plugin: "nav2_system_tests::SmoothedPathInCollision" + failed_to_smooth_path: + plugin: "nav2_system_tests::FailedToSmoothPath" + invalid_path: + plugin: "nav2_system_tests::InvalidPath" diff --git a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.cpp b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.cpp new file mode 100644 index 00000000000..265fd553106 --- /dev/null +++ b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.cpp @@ -0,0 +1,22 @@ +// 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 "smoother_error_plugin.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorSmoother, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimeOutErrorSmoother, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::SmoothedPathInCollision, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::FailedToSmoothPath, nav2_core::Smoother) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::InvalidPath, nav2_core::Smoother) diff --git a/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp new file mode 100644 index 00000000000..45ad7f469e2 --- /dev/null +++ b/nav2_system_tests/src/error_codes/smoother/smoother_error_plugin.hpp @@ -0,0 +1,92 @@ +// 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__SMOOTHER__SMOOTHER_ERROR_PLUGIN_HPP_ +#define ERROR_CODES__SMOOTHER__SMOOTHER_ERROR_PLUGIN_HPP_ + +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_core/smoother_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorSmoother : public nav2_core::Smoother +{ +public: + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() override {} + + void activate() override {} + + void deactivate() override {} + + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) override + { + throw nav2_core::SmootherException("Unknown Smoother exception"); + } +}; + +class TimeOutErrorSmoother : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::SmootherTimedOut("Smoother timedOut"); + } +}; + +class SmoothedPathInCollision : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::SmoothedPathInCollision("Smoother path in collision"); + } +}; + +class FailedToSmoothPath : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::FailedToSmoothPath("Failed to smooth path"); + } +}; + +class InvalidPath : public UnknownErrorSmoother +{ + bool smooth( + nav_msgs::msg::Path &, + const rclcpp::Duration &) + { + throw nav2_core::InvalidPath("Invalid path"); + } +}; + +} // namespace nav2_system_tests + +#endif // ERROR_CODES__SMOOTHER__SMOOTHER_ERROR_PLUGIN_HPP_ diff --git a/nav2_system_tests/src/error_codes/smoother_plugins.xml b/nav2_system_tests/src/error_codes/smoother_plugins.xml new file mode 100644 index 00000000000..fb11cad0b17 --- /dev/null +++ b/nav2_system_tests/src/error_codes/smoother_plugins.xml @@ -0,0 +1,22 @@ + + + This smoother produces a unknown exception. + + + This smoother produces a TimedOut exception. + + + This smoother produces a path in collision exception. + + + This smoother produces a failed to smooth exception. + + + This smoother produces an invalid path 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 aeaff1ac448..d167d0b00eb 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -21,13 +21,16 @@ from nav2_simple_commander.robot_navigator import BasicNavigator import rclpy -from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses +from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses, SmoothPath def main(argv=sys.argv[1:]): rclpy.init() time.sleep(5) + # Wait for lifecycle nodes to come up fully + time.sleep(10) + navigator = BasicNavigator() # Set our demo's initial pose @@ -35,7 +38,7 @@ def main(argv=sys.argv[1:]): 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 initial_pose.pose.position.x = -2.00 initial_pose.pose.position.y = -0.50 initial_pose.pose.orientation.z = 0.0 @@ -122,6 +125,36 @@ 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 compute path to pose error codes + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = navigator.get_clock().now().to_msg() + + pose.pose.position.x = -1.00 + pose.pose.position.y = -0.50 + pose.pose.orientation.z = 0.0 + pose.pose.orientation.w = 1.0 + + pose1 = pose + pose1.pose.position.x = -0.5 + + a_path = Path() + a_path.poses.append(pose) + a_path.poses.append(pose1) + + smoother = { + 'invalid_smoother': SmoothPath.Goal().INVALID_SMOOTHER, + 'unknown': SmoothPath.Goal().UNKNOWN, + 'timeout': SmoothPath.Goal().TIMEOUT, + 'smoothed_path_in_collision': SmoothPath.Goal().SMOOTHED_PATH_IN_COLLISION, + 'failed_to_smooth_path': SmoothPath.Goal().FAILED_TO_SMOOTH_PATH, + 'invalid_path': SmoothPath.Goal().INVALID_PATH + } + + for smoother, error_code in smoother.items(): + result = navigator._smoothPathImpl(a_path, smoother) + assert result.error_code == error_code, "Smoother error does not match" + 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 165e1806ce4..8a2ceb84819 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 @@ -31,7 +31,8 @@ def generate_launch_description(): ('/tf_static', 'tf_static')] lifecycle_nodes = ['controller_server', - 'planner_server'] + 'planner_server', + 'smoother_server'] load_nodes = GroupAction( actions=[ @@ -60,6 +61,14 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[params_file], remappings=remappings), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager',