diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index a5e4a539c02..2a7a8495f54 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -85,15 +85,16 @@ controller_server: movement_time_allowance: 10.0 # Goal checker parameters #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" + # plugin: "nav2_controller::PathCompleteGoalChecker" # xy_goal_tolerance: 0.25 # yaw_goal_tolerance: 0.25 # stateful: True general_goal_checker: stateful: True - plugin: "nav2_controller::SimpleGoalChecker" + plugin: "nav2_controller::PathCompleteGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + path_length_tolerance: 1.0 FollowPath: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 76725b8d47f..fb641a58e91 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -145,6 +145,27 @@ target_link_libraries(stopped_goal_checker PRIVATE pluginlib::pluginlib ) + +add_library(path_complete_goal_checker SHARED plugins/path_complete_goal_checker.cpp) +target_include_directories(path_complete_goal_checker + PUBLIC + "$" + "$" +) +target_link_libraries(path_complete_goal_checker PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + simple_goal_checker +) +target_link_libraries(path_complete_goal_checker PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -161,7 +182,13 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name} +install(TARGETS + simple_progress_checker + pose_progress_checker + simple_goal_checker + stopped_goal_checker + path_complete_goal_checker + ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -181,6 +208,7 @@ ament_export_libraries(simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker + path_complete_goal_checker ${library_name}) ament_export_dependencies( geometry_msgs diff --git a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp new file mode 100644 index 00000000000..9cd64bfd105 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp @@ -0,0 +1,82 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Joseph Duchesne + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ + +#include +#include +#include + +#include "nav2_controller/plugins/simple_goal_checker.hpp" + +namespace nav2_controller +{ + +/** + * @class PathCompleteGoalChecker + * @brief Goal Checker plugin that checks position delta, once path is shorter than a threshold. + */ +class PathCompleteGoalChecker : public SimpleGoalChecker +{ +public: + PathCompleteGoalChecker(); + + // Standard GoalChecker Interface + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) override; + + void reset() override; + + bool isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; + +protected: + // minimum remaining path length before checking position goals + double path_length_tolerance_; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; + +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 8bb3d9a949f..bcf73304aa1 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -64,9 +64,11 @@ class SimpleGoalChecker : public nav2_core::GoalChecker const std::string & plugin_name, const std::shared_ptr costmap_ros) override; void reset() override; + bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; + bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index e2f6ee0a465..65b715fe3b9 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -61,7 +61,7 @@ class StoppedGoalChecker : public SimpleGoalChecker const std::shared_ptr costmap_ros) override; bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) override; + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, geometry_msgs::msg::Twist & vel_tolerance) override; diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index ffbd0e5f1cb..e8aff7fe31d 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -19,4 +19,9 @@ Checks linear and angular velocity after stopping + + + Checks if path is below a threshold in length, and then if the current pose is within goal window for x, y and yaw + + diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp new file mode 100644 index 00000000000..3d45bba5c56 --- /dev/null +++ b/nav2_controller/plugins/path_complete_goal_checker.cpp @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Joseph Duchesne + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "nav2_controller/plugins/path_complete_goal_checker.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ + +PathCompleteGoalChecker::PathCompleteGoalChecker() +: SimpleGoalChecker(), path_length_tolerance_(1.0) +{ +} + +void PathCompleteGoalChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) +{ + SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros); + + auto node = parent.lock(); + + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".path_length_tolerance", + rclcpp::ParameterValue(path_length_tolerance_)); + + node->get_parameter(plugin_name + ".path_length_tolerance", path_length_tolerance_); + + // Replace SimpleGoalChecker's callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&PathCompleteGoalChecker::dynamicParametersCallback, this, _1)); +} + +void PathCompleteGoalChecker::reset() +{ + SimpleGoalChecker::reset(); +} + +bool PathCompleteGoalChecker::isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & twist, const nav_msgs::msg::Path & path) +{ + using nav2_util::geometry_utils::is_path_longer_than_length; + + if (is_path_longer_than_length(path, path_length_tolerance_)) { + return false; + } + // otherwise defer to SimpleGoalChecker's isGoalReached + return SimpleGoalChecker::isGoalReached(query_pose, goal_pose, twist, path); +} + +rcl_interfaces::msg::SetParametersResult +PathCompleteGoalChecker::dynamicParametersCallback(std::vector parameters) +{ + auto result = rcl_interfaces::msg::SetParametersResult(); + + for (auto & parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".path_length_tolerance") { + path_length_tolerance_ = parameter.as_double(); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::PathCompleteGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 30b90f4f4ab..979c3bcc720 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -97,7 +97,7 @@ void SimpleGoalChecker::reset() bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &) + const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &) { if (check_xy_) { double dx = query_pose.position.x - goal_pose.position.x, diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index 179c698bd46..f13b3afc7c2 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -82,9 +82,9 @@ void StoppedGoalChecker::initialize( bool StoppedGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) + const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) { - bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity); + bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, current_path); if (!ret) { return ret; } diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index cd8048513d4..6d08b0015bb 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -2,4 +2,4 @@ ament_add_gtest(pctest progress_checker.cpp) target_link_libraries(pctest simple_progress_checker pose_progress_checker nav_2d_utils::conversions) ament_add_gtest(gctest goal_checker.cpp) -target_link_libraries(gctest simple_goal_checker stopped_goal_checker nav_2d_utils::conversions) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker path_complete_goal_checker nav_2d_utils::conversions) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index acfffcb9cd9..d25564004c8 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -38,17 +38,21 @@ #include "gtest/gtest.h" #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_controller/plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/path_complete_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_util/lifecycle_node.hpp" using nav2_controller::SimpleGoalChecker; using nav2_controller::StoppedGoalChecker; +using nav2_controller::PathCompleteGoalChecker; void checkMacro( nav2_core::GoalChecker & gc, double x0, double y0, double theta0, double x1, double y1, double theta1, double xv, double yv, double thetav, + const nav_msgs::msg::Path & path, bool expected_result) { gc.reset(); @@ -67,35 +71,15 @@ void checkMacro( EXPECT_TRUE( gc.isGoalReached( nav_2d_utils::pose2DToPose(pose0), - nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v))); + nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v), path)); } else { EXPECT_FALSE( gc.isGoalReached( nav_2d_utils::pose2DToPose(pose0), - nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v))); + nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v), path)); } } -void sameResult( - nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1, - double x0, double y0, double theta0, - double x1, double y1, double theta1, - double xv, double yv, double thetav, - bool expected_result) -{ - checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result); - checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result); -} - -void trueFalse( - nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1, - double x0, double y0, double theta0, - double x1, double y1, double theta1, - double xv, double yv, double thetav) -{ - checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true); - checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false); -} class TestLifecycleNode : public nav2_util::LifecycleNode { public: @@ -135,7 +119,7 @@ class TestLifecycleNode : public nav2_util::LifecycleNode } }; -TEST(VelocityIterator, goal_checker_reset) +TEST(SimpleGoalChecker, goal_checker_reset) { auto x = std::make_shared("goal_checker"); @@ -145,7 +129,7 @@ TEST(VelocityIterator, goal_checker_reset) EXPECT_TRUE(true); } -TEST(VelocityIterator, stopped_goal_checker_reset) +TEST(StoppedGoalChecker, stopped_goal_checker_reset) { auto x = std::make_shared("stopped_goal_checker"); @@ -155,45 +139,122 @@ TEST(VelocityIterator, stopped_goal_checker_reset) EXPECT_TRUE(true); } -TEST(VelocityIterator, two_checks) +TEST(StoppedGoalChecker, path_complete_goal_checker_reset) +{ + auto x = std::make_shared("path_complete_goal_checker"); + + nav2_core::GoalChecker * pcgc = new PathCompleteGoalChecker; + pcgc->reset(); + delete pcgc; + EXPECT_TRUE(true); +} + + +TEST(SimpleGoalChecker, test_goal_checking) { auto x = std::make_shared("goal_checker"); SimpleGoalChecker gc; + auto costmap = std::make_shared("test_costmap"); + nav_msgs::msg::Path path; + gc.initialize(x, "nav2_controller", costmap); + + checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); + checkMacro(gc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); + checkMacro(gc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); + checkMacro(gc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); + checkMacro(gc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); + checkMacro(gc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, true); + checkMacro(gc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, true); + checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, true); + checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, true); +} + +TEST(StoppedGoalChecker, test_goal_checking) +{ + auto x = std::make_shared("goal_checker"); + StoppedGoalChecker sgc; auto costmap = std::make_shared("test_costmap"); + nav_msgs::msg::Path path; - gc.initialize(x, "nav2_controller", costmap); sgc.initialize(x, "nav2_controller", costmap); - sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); - sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); - sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); - sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false); - sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true); - trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1); + + checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); + checkMacro(sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); + checkMacro(sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); + checkMacro(sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); + checkMacro(sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); + checkMacro(sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, false); + checkMacro(sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, false); + checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, false); + checkMacro(sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, false); + + // todo: add some where path complete goal checker differs +} + +TEST(PathCompleteGoalChecker, test_goal_checking) +{ + auto x = std::make_shared("goal_checker"); + + PathCompleteGoalChecker pcgc; + auto costmap = std::make_shared("test_costmap"); + nav_msgs::msg::Path path; + pcgc.initialize(x, "nav2_controller", costmap); + + // Add one pose + { + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose.position.x = 0.0; + path.poses.push_back(pose_stamped_msg); + } + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); + checkMacro(pcgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); + checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, true); + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, true); + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, true); + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, true); + + // add a second pose at {2.0,0}, making the total path length 2.0m + // this should prevent any completions due to path_length_tolerance=1.0 + { + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose.position.x = 2.0; + path.poses.push_back(pose_stamped_msg); + } + + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path, false); + checkMacro(pcgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path, false); } + TEST(StoppedGoalChecker, get_tol_and_dynamic_params) { auto x = std::make_shared("goal_checker"); - SimpleGoalChecker gc; StoppedGoalChecker sgc; auto costmap = std::make_shared("test_costmap"); sgc.initialize(x, "test", costmap); - gc.initialize(x, "test2", costmap); geometry_msgs::msg::Pose pose_tol; geometry_msgs::msg::Twist vel_tol; // Test stopped goal checker's tolerance API EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); - EXPECT_EQ(vel_tol.linear.x, 0.25); - EXPECT_EQ(vel_tol.linear.y, 0.25); - EXPECT_EQ(vel_tol.angular.z, 0.25); + EXPECT_EQ(pose_tol.position.x, 0.25); + EXPECT_EQ(pose_tol.position.y, 0.25); + auto p2d = nav_2d_utils::poseToPose2D(pose_tol); + EXPECT_NEAR(p2d.theta, 0.25, 1e-6); // Test Stopped goal checker's dynamic parameters auto rec_param = std::make_shared( @@ -212,8 +273,37 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0); EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0); + + // Test the dynamic parameters impacted the tolerances + EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(vel_tol.linear.x, 100.0); + EXPECT_EQ(vel_tol.linear.y, 100.0); + EXPECT_EQ(vel_tol.angular.z, 100.0); +} + + +TEST(SimpleGoalChecker, get_tol_and_dynamic_params) +{ + auto x = std::make_shared("goal_checker"); + + SimpleGoalChecker gc; + auto costmap = std::make_shared("test_costmap"); + + gc.initialize(x, "test2", costmap); + geometry_msgs::msg::Pose pose_tol; + geometry_msgs::msg::Twist vel_tol; + + // Test stopped goal checker's tolerance API + EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(pose_tol.position.x, 0.25); + EXPECT_EQ(pose_tol.position.y, 0.25); + // Test normal goal checker's dynamic parameters - results = rec_param->set_parameters_atomically( + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), rclcpp::Parameter("test2.stateful", true)}); @@ -227,16 +317,53 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); // Test the dynamic parameters impacted the tolerances - EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); - EXPECT_EQ(vel_tol.linear.x, 100.0); - EXPECT_EQ(vel_tol.linear.y, 100.0); - EXPECT_EQ(vel_tol.angular.z, 100.0); - EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); EXPECT_EQ(pose_tol.position.x, 200.0); EXPECT_EQ(pose_tol.position.y, 200.0); } +TEST(PathCompleteGoalChecker, get_tol_and_dynamic_params) +{ + auto x = std::make_shared("goal_checker"); + + PathCompleteGoalChecker pcgc; + auto costmap = std::make_shared("test_costmap"); + + pcgc.initialize(x, "test3", costmap); + geometry_msgs::msg::Pose pose_tol; + geometry_msgs::msg::Twist vel_tol; + + // Test stopped goal checker's tolerance API + EXPECT_TRUE(pcgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(pose_tol.position.x, 0.25); + EXPECT_EQ(pose_tol.position.y, 0.25); + + // Test normal goal checker's dynamic parameters + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test3.xy_goal_tolerance", 200.0), + rclcpp::Parameter("test3.yaw_goal_tolerance", 200.0), + rclcpp::Parameter("test3.path_length_tolerance", 3.0), + rclcpp::Parameter("test3.stateful", true)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ(x->get_parameter("test3.xy_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test3.yaw_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test3.path_length_tolerance").as_double(), 3.0); + EXPECT_EQ(x->get_parameter("test3.stateful").as_bool(), true); + + // Test the dynamic parameters impacted the tolerances + EXPECT_TRUE(pcgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(pose_tol.position.x, 200.0); + EXPECT_EQ(pose_tol.position.y, 200.0); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 57003871a4e..e0dc66d0676 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -41,7 +41,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) default_progress_checker_types_{"nav2_controller::SimpleProgressChecker"}, goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), default_goal_checker_ids_{"goal_checker"}, - default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}, + default_goal_checker_types_{"nav2_controller::PathCompleteGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"}, @@ -786,7 +786,7 @@ bool ControllerServer::isGoalReached() return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, - velocity); + velocity, current_path_); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index 03e11c9a3e9..23a2055b44d 100644 --- a/nav2_core/include/nav2_core/goal_checker.hpp +++ b/nav2_core/include/nav2_core/goal_checker.hpp @@ -42,6 +42,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -84,8 +85,10 @@ class GoalChecker * @return True if goal is reached */ virtual bool isGoalReached( - const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist & velocity) = 0; + const geometry_msgs::msg::Pose & query_pose, + const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & velocity, + const nav_msgs::msg::Path & current_path) = 0; /** * @brief Get the maximum possible tolerances used for goal checking in the major types. diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index bcb3d97be45..40f5f5a6649 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -53,7 +53,8 @@ class TestGoalChecker : public nav2_core::GoalChecker virtual bool isGoalReached( const geometry_msgs::msg::Pose & /*query_pose*/, const geometry_msgs::msg::Pose & /*goal_pose*/, - const geometry_msgs::msg::Twist & /*velocity*/) {return false;} + const geometry_msgs::msg::Twist & /*velocity*/, + const nav_msgs::msg::Path & /*current_path*/) {return false;} virtual bool getTolerances( geometry_msgs::msg::Pose & pose_tolerance, diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 81244d804df..cbf6c4c9b36 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -110,7 +110,7 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" + plugin: "nav2_controller::PathCompleteGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 675a2aefe3c..58a96bc2d2c 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -57,7 +57,7 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" + plugin: "nav2_controller::PathCompleteGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 67cc87243d6..ddad19fc8bc 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -179,6 +179,34 @@ inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t sta return path_length; } +/** + * @brief Check if path is longer than a specific lenght. + * Similar to calculate_path_length(path), but short-circuits when length is exceeded. + * @param path Path containing the poses that are planned + * @param length The length we're checking against + * @return bool True if path is longer than length, false if path <= length + */ +inline bool is_path_longer_than_length(const nav_msgs::msg::Path & path, const double length) +{ + if (length < 0.0) { + return true; // a path must be longer than a negative length + } + if (path.poses.size() == 0) { + return false; // empty path can't be longer than length >= 0.0 + } + size_t check_count = path.poses.size() - 1; + double path_length = 0.0; + for (size_t idx = 0; idx < check_count; ++idx) { + path_length += geometry_utils::euclidean_distance(path.poses[idx].pose, + path.poses[idx + 1].pose); + if (path_length > length) { + return true; + } + } + return false; +} + + } // namespace geometry_utils } // namespace nav2_util diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 38f27ad3044..3c92ddb7199 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -13,6 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -21,6 +22,7 @@ using nav2_util::geometry_utils::euclidean_distance; using nav2_util::geometry_utils::calculate_path_length; +using nav2_util::geometry_utils::is_path_longer_than_length; TEST(GeometryUtils, euclidean_distance_point_3d) { @@ -128,3 +130,74 @@ TEST(GeometryUtils, calculate_path_length) calculate_path_length(circle_path), 2 * pi * polar_distance, 1e-1); } + +TEST(GeometryUtils, is_path_longer_than_length) +{ + nav_msgs::msg::Path empty_path; + ASSERT_FALSE(is_path_longer_than_length(empty_path, 0.0)); + ASSERT_FALSE(is_path_longer_than_length(empty_path, 1e-9)); + ASSERT_FALSE(is_path_longer_than_length(empty_path, 1.0)); + // unclear why we have a negative length, but it should work anyway + ASSERT_TRUE(is_path_longer_than_length(empty_path, -1.0)); + + // a single pose path should be equivalent to empty_path above, since it also has no length + nav_msgs::msg::Path single_pose_path; + { + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose.position.x = 2.0; + single_pose_path.poses.push_back(pose_stamped_msg); + } + ASSERT_FALSE(is_path_longer_than_length(single_pose_path, 0.0)); + ASSERT_FALSE(is_path_longer_than_length(single_pose_path, 1e-9)); + ASSERT_FALSE(is_path_longer_than_length(single_pose_path, 1.0)); + ASSERT_TRUE(is_path_longer_than_length(single_pose_path, -1.0)); + + nav_msgs::msg::Path straight_line_path; + size_t nb_path_points = 10; + float distance_between_poses = 2.0; + float current_x_loc = 0.0; + + for (size_t i = 0; i < nb_path_points; ++i) { + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose.position.x = current_x_loc; + + straight_line_path.poses.push_back(pose_stamped_msg); + + current_x_loc += distance_between_poses; + } + + ASSERT_TRUE(is_path_longer_than_length(straight_line_path, -3.14)); + ASSERT_TRUE(is_path_longer_than_length(straight_line_path, 0.0)); + ASSERT_TRUE(is_path_longer_than_length(straight_line_path, 1.0)); + ASSERT_TRUE(is_path_longer_than_length(straight_line_path, 5.0)); + ASSERT_TRUE(is_path_longer_than_length(straight_line_path, 9.999)); + ASSERT_TRUE(is_path_longer_than_length(straight_line_path, 15.0)); + ASSERT_TRUE(is_path_longer_than_length(straight_line_path, 17.9999)); + + ASSERT_FALSE(is_path_longer_than_length(straight_line_path, 18.0)); + ASSERT_FALSE(is_path_longer_than_length(straight_line_path, 1e7)); + ASSERT_FALSE(is_path_longer_than_length(straight_line_path, + std::numeric_limits::infinity())); + + nav_msgs::msg::Path circle_path; + float polar_distance = 2.0; + uint32_t current_polar_angle_deg = 0; + constexpr float pi = 3.14159265358979; + + while (current_polar_angle_deg != 360) { + float x_loc = polar_distance * std::cos(current_polar_angle_deg * (pi / 180.0)); + float y_loc = polar_distance * std::sin(current_polar_angle_deg * (pi / 180.0)); + + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose.position.x = x_loc; + pose_stamped_msg.pose.position.y = y_loc; + + circle_path.poses.push_back(pose_stamped_msg); + + current_polar_angle_deg += 1; + } + + ASSERT_TRUE(is_path_longer_than_length(circle_path, 0.0)); + ASSERT_TRUE(is_path_longer_than_length(circle_path, 2.0 * 2.0 * pi - 1.0)); + ASSERT_FALSE(is_path_longer_than_length(circle_path, 2.0 * 2.0 * pi)); +}