From d0e22c24fdec670ce3f727dfa8047d5becb67d99 Mon Sep 17 00:00:00 2001 From: Jonas Otto Date: Sun, 4 Aug 2024 12:46:23 +0200 Subject: [PATCH 01/12] add path argument to goal checker interface Signed-off-by: Jonas Otto --- .../plugins/simple_goal_checker.hpp | 2 +- .../plugins/stopped_goal_checker.hpp | 2 +- .../plugins/simple_goal_checker.cpp | 2 +- .../plugins/stopped_goal_checker.cpp | 4 +- nav2_controller/plugins/test/goal_checker.cpp | 37 +++++++++++-------- nav2_controller/src/controller_server.cpp | 2 +- nav2_core/include/nav2_core/goal_checker.hpp | 7 +++- nav2_mppi_controller/test/utils_test.cpp | 3 +- 8 files changed, 34 insertions(+), 25 deletions(-) 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 e9091db9ae8..f03f0b74880 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -66,7 +66,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker 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 d7586db6f31..dd6e39d57d5 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/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 597f843eaac..657a2e8777f 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/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index acfffcb9cd9..16c63b2cbab 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -39,6 +39,7 @@ #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_controller/plugins/stopped_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; @@ -49,6 +50,7 @@ void checkMacro( 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,12 +69,12 @@ 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)); } } @@ -81,20 +83,22 @@ void sameResult( 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) { - 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); + checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, expected_result); + checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, 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) + double xv, double yv, double thetav, + const nav_msgs::msg::Path & path) { - checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true); - checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false); + checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, true); + checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, false); } class TestLifecycleNode : public nav2_util::LifecycleNode { @@ -162,18 +166,19 @@ TEST(VelocityIterator, two_checks) SimpleGoalChecker gc; 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); + sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true); + sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); + sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); + sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); + sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); + trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path); + trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path); + trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path); + trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path); } TEST(StoppedGoalChecker, get_tol_and_dynamic_params) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 93e66539392..dcef3c6fa2b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -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 4c939bd72b0..529ed2e7c0c 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, From 55a9e68b2bc13170054ad20f82c6942f5b04ed51 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 10 Dec 2024 02:46:04 +0000 Subject: [PATCH 02/12] New PathCompleteGoalChecker controller plugin Signed-off-by: Joseph Duchesne --- nav2_controller/CMakeLists.txt | 23 ++++ .../plugins/path_complete_goal_checker.hpp | 84 +++++++++++++ .../plugins/simple_goal_checker.hpp | 2 + nav2_controller/plugins.xml | 5 + .../plugins/path_complete_goal_checker.cpp | 111 ++++++++++++++++++ nav2_controller/plugins/test/CMakeLists.txt | 2 +- 6 files changed, 226 insertions(+), 1 deletion(-) create mode 100644 nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp create mode 100644 nav2_controller/plugins/path_complete_goal_checker.cpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 76725b8d47f..8f3b41e0b19 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -145,6 +145,28 @@ 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} +) +target_link_libraries(path_complete_goal_checker PRIVATE + angles::angles + nav2_util::nav2_util_core + pluginlib::pluginlib + tf2::tf2 +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -181,6 +203,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..ca7edf24831 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp @@ -0,0 +1,84 @@ +/* + * 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. + * + * A + */ +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: + // threshold for path goal + int path_length_tolerence_; + + /** + * @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 f03f0b74880..0ad6e667b8e 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, 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..8aa2487f180 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 has completed enough poses, and then if the 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..56634bec5fe --- /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" + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ + +PathCompleteGoalChecker::PathCompleteGoalChecker() +: SimpleGoalChecker(), path_length_tolerence_(1) +{ +} + +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_tolerence", + rclcpp::ParameterValue(path_length_tolerence_)); + + node->get_parameter(plugin_name + ".path_length_tolerence", path_length_tolerence_); + + // Replace SimpleGoalChecker's callback for dynamic parameters + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + 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) +{ + // return false if more than path_length_tolerence_ waypoints exist + // note: another useful version of this could check path length + if (path.poses.size() > (unsigned int)path_length_tolerence_) { + 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) +{ + // call the base class (might be unnessesary since the base class will already bind this event) + rcl_interfaces::msg::SetParametersResult result = + SimpleGoalChecker::dynamicParametersCallback(parameters); + for (auto & parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_INTEGER) { + if (name == plugin_name_ + ".path_length_tolerence") { + path_length_tolerence_ = parameter.as_int(); + } + } + } + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::PathCompleteGoalChecker, nav2_core::GoalChecker) 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) From 524ab92a5a31960e1ebdcb7e963f0da4cf80646e Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 10 Dec 2024 02:48:29 +0000 Subject: [PATCH 03/12] - Split goal checker plugin tests into per-goal-checker tests, since checking 3 goal checkers at once in a single method proved too unweildy. - Added tests for the new PathCompleteGoalChecker Signed-off-by: Joseph Duchesne --- nav2_controller/plugins/test/goal_checker.cpp | 207 ++++++++++++++---- 1 file changed, 161 insertions(+), 46 deletions(-) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 16c63b2cbab..04ca781bf64 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -38,12 +38,14 @@ #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, @@ -78,28 +80,6 @@ void checkMacro( } } -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, - const nav_msgs::msg::Path & path, - bool expected_result) -{ - checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, expected_result); - checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, 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, - const nav_msgs::msg::Path & path) -{ - checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, true); - checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, false); -} class TestLifecycleNode : public nav2_util::LifecycleNode { public: @@ -139,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"); @@ -149,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"); @@ -159,46 +139,115 @@ 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, path, true); - sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false); - sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false); - sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false); - sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true); - trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path); - trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path); + + 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 default constructed pose to the path + // this should have no impact on the results vrs. SimpleGoalChecker + path.poses.emplace_back(); + 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 default constructed pose to the path + // this should prevent any completions due to path_length_tolerence=1 + path.poses.emplace_back(); + + 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( @@ -217,8 +266,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)}); @@ -232,16 +310,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_tolerence", 3), + 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_tolerence").as_int(), 3); + 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); From d90135ea9520eb28323eebec832a535851dace06 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 10 Dec 2024 03:17:09 +0000 Subject: [PATCH 04/12] Fixed a silly spelling error Signed-off-by: Joseph Duchesne --- .../plugins/path_complete_goal_checker.hpp | 2 +- .../plugins/path_complete_goal_checker.cpp | 16 ++++++++-------- nav2_controller/plugins/test/goal_checker.cpp | 6 +++--- 3 files changed, 12 insertions(+), 12 deletions(-) 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 index ca7edf24831..cfb2c728e49 100644 --- a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp @@ -69,7 +69,7 @@ class PathCompleteGoalChecker : public SimpleGoalChecker protected: // threshold for path goal - int path_length_tolerence_; + int path_length_tolerance_; /** * @brief Callback executed when a paramter change is detected diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp index 56634bec5fe..3dd153cf3f6 100644 --- a/nav2_controller/plugins/path_complete_goal_checker.cpp +++ b/nav2_controller/plugins/path_complete_goal_checker.cpp @@ -43,7 +43,7 @@ namespace nav2_controller { PathCompleteGoalChecker::PathCompleteGoalChecker() -: SimpleGoalChecker(), path_length_tolerence_(1) +: SimpleGoalChecker(), path_length_tolerance_(1) { } @@ -58,10 +58,10 @@ void PathCompleteGoalChecker::initialize( nav2_util::declare_parameter_if_not_declared( node, - plugin_name + ".path_length_tolerence", - rclcpp::ParameterValue(path_length_tolerence_)); + plugin_name + ".path_length_tolerance", + rclcpp::ParameterValue(path_length_tolerance_)); - node->get_parameter(plugin_name + ".path_length_tolerence", path_length_tolerence_); + node->get_parameter(plugin_name + ".path_length_tolerance", path_length_tolerance_); // Replace SimpleGoalChecker's callback for dynamic parameters node->remove_on_set_parameters_callback(dyn_params_handler_.get()); @@ -78,9 +78,9 @@ 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) { - // return false if more than path_length_tolerence_ waypoints exist + // return false if more than path_length_tolerance_ waypoints exist // note: another useful version of this could check path length - if (path.poses.size() > (unsigned int)path_length_tolerence_) { + if (path.poses.size() > (unsigned int)path_length_tolerance_) { return false; } // otherwise defer to SimpleGoalChecker's isGoalReached @@ -98,8 +98,8 @@ PathCompleteGoalChecker::dynamicParametersCallback(std::vectorset_parameters_atomically( {rclcpp::Parameter("test3.xy_goal_tolerance", 200.0), rclcpp::Parameter("test3.yaw_goal_tolerance", 200.0), - rclcpp::Parameter("test3.path_length_tolerence", 3), + rclcpp::Parameter("test3.path_length_tolerance", 3), rclcpp::Parameter("test3.stateful", true)}); rclcpp::spin_until_future_complete( @@ -348,7 +348,7 @@ TEST(PathCompleteGoalChecker, get_tol_and_dynamic_params) 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_tolerence").as_int(), 3); + EXPECT_EQ(x->get_parameter("test3.path_length_tolerance").as_int(), 3); EXPECT_EQ(x->get_parameter("test3.stateful").as_bool(), true); // Test the dynamic parameters impacted the tolerances From 63aecac4764ce0cbc366f112bb5806bde1d8bf98 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 10 Dec 2024 04:19:32 +0000 Subject: [PATCH 05/12] Adding missing install target for new plugin Signed-off-by: Joseph Duchesne --- nav2_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 8f3b41e0b19..8b754fa634e 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -183,7 +183,7 @@ 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 From 5d86e1c6dba079bf5107b5980c06841451c50b51 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 11 Dec 2024 03:29:38 +0000 Subject: [PATCH 06/12] Fixed typo and lint error Signed-off-by: Joseph Duchesne --- nav2_controller/CMakeLists.txt | 8 +++++++- .../plugins/path_complete_goal_checker.hpp | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 8b754fa634e..da1acdc9c9f 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -183,7 +183,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 path_complete_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 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 index cfb2c728e49..a7b7a9e2585 100644 --- a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp @@ -48,7 +48,7 @@ namespace nav2_controller * @class PathCompleteGoalChecker * @brief Goal Checker plugin that checks position delta, once path is shorter than a threshold. * - * A + * */ class PathCompleteGoalChecker : public SimpleGoalChecker { From 488376c3c22f57ce70df962e8174beab638c58f4 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 11 Dec 2024 04:40:05 +0000 Subject: [PATCH 07/12] Added is_path_longer_than_length() utility method and unit tests. Signed-off-by: Joseph Duchesne --- .../include/nav2_util/geometry_utils.hpp | 28 +++++++++ nav2_util/test/test_geometry_utils.cpp | 61 +++++++++++++++++++ 2 files changed, 89 insertions(+) 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..2a2c8a74b99 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,62 @@ 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)); + + 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)); +} From 5988e0d9c13ce682580a5fee9b9c6405b7ef296c Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 11 Dec 2024 06:10:15 +0000 Subject: [PATCH 08/12] Changed PathCompleteGoalChecker to measure remaining path length as a threshold rather than pose count. Extended tests. Signed-off-by: Joseph Duchesne --- nav2_controller/CMakeLists.txt | 2 +- .../plugins/path_complete_goal_checker.hpp | 4 ++-- .../plugins/path_complete_goal_checker.cpp | 13 ++++++----- nav2_controller/plugins/test/goal_checker.cpp | 23 ++++++++++++------- nav2_util/test/test_geometry_utils.cpp | 12 ++++++++++ 5 files changed, 37 insertions(+), 17 deletions(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index da1acdc9c9f..8fb4fbc7419 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -183,7 +183,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS +install(TARGETS simple_progress_checker pose_progress_checker simple_goal_checker 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 index a7b7a9e2585..54e977a8eec 100644 --- a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp @@ -68,8 +68,8 @@ class PathCompleteGoalChecker : public SimpleGoalChecker const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override; protected: - // threshold for path goal - int path_length_tolerance_; + // minimum remaining path length before checking position goals + double path_length_tolerance_; /** * @brief Callback executed when a paramter change is detected diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp index 3dd153cf3f6..0d60b190c54 100644 --- a/nav2_controller/plugins/path_complete_goal_checker.cpp +++ b/nav2_controller/plugins/path_complete_goal_checker.cpp @@ -35,6 +35,7 @@ #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; @@ -43,7 +44,7 @@ namespace nav2_controller { PathCompleteGoalChecker::PathCompleteGoalChecker() -: SimpleGoalChecker(), path_length_tolerance_(1) +: SimpleGoalChecker(), path_length_tolerance_(1.0) { } @@ -78,9 +79,9 @@ 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) { - // return false if more than path_length_tolerance_ waypoints exist - // note: another useful version of this could check path length - if (path.poses.size() > (unsigned int)path_length_tolerance_) { + 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 @@ -97,9 +98,9 @@ PathCompleteGoalChecker::dynamicParametersCallback(std::vectorset_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), + rclcpp::Parameter("test3.path_length_tolerance", 3.0), rclcpp::Parameter("test3.stateful", true)}); rclcpp::spin_until_future_complete( @@ -348,7 +355,7 @@ TEST(PathCompleteGoalChecker, get_tol_and_dynamic_params) 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_int(), 3); + 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 diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 2a2c8a74b99..3c92ddb7199 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -140,6 +140,18 @@ TEST(GeometryUtils, is_path_longer_than_length) // 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; From 01c1b2b3910075e703fda9d4c79bffa51b519061 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 11 Dec 2024 06:58:50 +0000 Subject: [PATCH 09/12] Updated default GoalChecker to be PathCompleteGoalChecker Signed-off-by: Joseph Duchesne --- nav2_bringup/params/nav2_params.yaml | 5 +++-- nav2_controller/plugins.xml | 2 +- nav2_controller/src/controller_server.cpp | 2 +- nav2_regulated_pure_pursuit_controller/README.md | 2 +- nav2_rotation_shim_controller/README.md | 2 +- 5 files changed, 7 insertions(+), 6 deletions(-) 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/plugins.xml b/nav2_controller/plugins.xml index 8aa2487f180..e8aff7fe31d 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -21,7 +21,7 @@ - Checks if path has completed enough poses, and then if the pose is within goal window for x, y and yaw + 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/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index dcef3c6fa2b..e8a6ff10c42 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"}, diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index acdd0eb1ed9..83c6ba0f993 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 From 8c1b56092a163aa5a5ca150fdaa8b0b91e620a4c Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 11 Dec 2024 07:44:54 +0000 Subject: [PATCH 10/12] Added missing link library for path_complete_goal_checker Signed-off-by: Joseph Duchesne --- nav2_controller/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 8fb4fbc7419..fb641a58e91 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -159,12 +159,11 @@ target_link_libraries(path_complete_goal_checker PUBLIC rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ${rcl_interfaces_TARGETS} + simple_goal_checker ) target_link_libraries(path_complete_goal_checker PRIVATE - angles::angles nav2_util::nav2_util_core pluginlib::pluginlib - tf2::tf2 ) if(BUILD_TESTING) From 7ba08df37e4e7c0a55e67e3187476c09767ce97d Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 15 Jan 2025 03:08:59 +0000 Subject: [PATCH 11/12] Removed extra empty comment lines, and fixed redundant/misunderstood dynamic param callack code Signed-off-by: Joseph Duchesne --- .../nav2_controller/plugins/path_complete_goal_checker.hpp | 2 -- nav2_controller/plugins/path_complete_goal_checker.cpp | 4 ---- 2 files changed, 6 deletions(-) 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 index 54e977a8eec..9cd64bfd105 100644 --- a/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp @@ -47,8 +47,6 @@ namespace nav2_controller /** * @class PathCompleteGoalChecker * @brief Goal Checker plugin that checks position delta, once path is shorter than a threshold. - * - * */ class PathCompleteGoalChecker : public SimpleGoalChecker { diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp index 0d60b190c54..b6604d7dd00 100644 --- a/nav2_controller/plugins/path_complete_goal_checker.cpp +++ b/nav2_controller/plugins/path_complete_goal_checker.cpp @@ -65,7 +65,6 @@ void PathCompleteGoalChecker::initialize( node->get_parameter(plugin_name + ".path_length_tolerance", path_length_tolerance_); // Replace SimpleGoalChecker's callback for dynamic parameters - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&PathCompleteGoalChecker::dynamicParametersCallback, this, _1)); } @@ -91,9 +90,6 @@ bool PathCompleteGoalChecker::isGoalReached( rcl_interfaces::msg::SetParametersResult PathCompleteGoalChecker::dynamicParametersCallback(std::vector parameters) { - // call the base class (might be unnessesary since the base class will already bind this event) - rcl_interfaces::msg::SetParametersResult result = - SimpleGoalChecker::dynamicParametersCallback(parameters); for (auto & parameter : parameters) { const auto & type = parameter.get_type(); const auto & name = parameter.get_name(); From 8626e9bb2735f2b7ea47fe4ef1521d98dc634ace Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 15 Jan 2025 04:02:24 +0000 Subject: [PATCH 12/12] Fixed missing return value Signed-off-by: Joseph Duchesne --- nav2_controller/plugins/path_complete_goal_checker.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nav2_controller/plugins/path_complete_goal_checker.cpp b/nav2_controller/plugins/path_complete_goal_checker.cpp index b6604d7dd00..3d45bba5c56 100644 --- a/nav2_controller/plugins/path_complete_goal_checker.cpp +++ b/nav2_controller/plugins/path_complete_goal_checker.cpp @@ -90,6 +90,8 @@ bool PathCompleteGoalChecker::isGoalReached( 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(); @@ -100,6 +102,7 @@ PathCompleteGoalChecker::dynamicParametersCallback(std::vector