diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index a3d2e7fe18c..6c655d4c86b 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -169,6 +169,27 @@ target_link_libraries(position_goal_checker PRIVATE pluginlib::pluginlib ) +add_library(axis_goal_checker SHARED plugins/axis_goal_checker.cpp) +target_include_directories(axis_goal_checker + PUBLIC + "$" + "$" +) +target_link_libraries(axis_goal_checker PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_ros_common::nav2_ros_common + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} +) +target_link_libraries(axis_goal_checker PRIVATE + angles::angles + nav2_util::nav2_util_core + pluginlib::pluginlib +) + add_library(feasible_path_handler SHARED plugins/feasible_path_handler.cpp) target_include_directories(feasible_path_handler PUBLIC @@ -211,6 +232,7 @@ install(TARGETS pose_progress_checker simple_goal_checker stopped_goal_checker + axis_goal_checker feasible_path_handler ${library_name} EXPORT ${library_name} @@ -233,6 +255,7 @@ ament_export_libraries(simple_progress_checker simple_goal_checker stopped_goal_checker position_goal_checker + axis_goal_checker feasible_path_handler ${library_name}) ament_export_dependencies( diff --git a/nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp new file mode 100644 index 00000000000..93d3ea221f9 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp @@ -0,0 +1,106 @@ +// Copyright (c) 2025 Dexory +// +// 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_CONTROLLER__PLUGINS__AXIS_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__AXIS_GOAL_CHECKER_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_core/goal_checker.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +namespace nav2_controller +{ + +/** + * @class AxisGoalChecker + * @brief Goal Checker plugin that checks progress along the axis defined by the last segment + * of the path to the goal. + * + * This class can be configured to allow overshoot past the goal if the is_overshoot_valid + * parameter is set to true (which is false by default). + */ +class AxisGoalChecker : public nav2_core::GoalChecker +{ +public: + /** + * @brief Construct a new Axis Goal Checker object + */ + AxisGoalChecker(); + + // Standard GoalChecker Interface + /** + * @brief Initialize the goal checker + * @param parent Weak pointer to the lifecycle node + * @param plugin_name Name of the plugin + * @param costmap_ros Shared pointer to the costmap + */ + void initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) override; + + /** + * @brief Reset the goal checker state + */ + void reset() override; + + /** + * @brief Check if the goal is reached + * @param query_pose Current pose of the robot + * @param goal_pose Target goal pose + * @param velocity Current velocity of the robot + * @param transformed_global_plan The transformed global plan + * @return true if goal is reached, false otherwise + */ + 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 & transformed_global_plan) override; + + /** + * @brief Get the position and velocity tolerances + * @param pose_tolerance Output parameter for pose tolerance + * @param vel_tolerance Output parameter for velocity tolerance + * @return true if tolerances are available, false otherwise + */ + bool getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) override; + +protected: + double along_path_tolerance_; + double cross_track_tolerance_; + double path_length_tolerance_; + bool is_overshoot_valid_; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::string plugin_name_; + rclcpp::Logger logger_{rclcpp::get_logger("AxisGoalChecker")}; + + /** + * @brief Callback executed when a parameter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; + +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__AXIS_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index a8be6497e12..a1bef56e9ff 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -24,6 +24,11 @@ Goal checker that only checks XY position and ignores orientation + + + Goal checker that checks progress along the axis defined by the last 2 poses of the path + + Prunes the global plan around the robot and transforms the remaining portion into the odom frame for local control diff --git a/nav2_controller/plugins/axis_goal_checker.cpp b/nav2_controller/plugins/axis_goal_checker.cpp new file mode 100644 index 00000000000..b417582f53d --- /dev/null +++ b/nav2_controller/plugins/axis_goal_checker.cpp @@ -0,0 +1,200 @@ +// Copyright (c) 2025 Dexory +// +// 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 +#include +#include +#include + +#include "angles/angles.h" +#include "nav2_controller/plugins/axis_goal_checker.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ + +AxisGoalChecker::AxisGoalChecker() +: along_path_tolerance_(0.25), cross_track_tolerance_(0.25), + path_length_tolerance_(1.0), is_overshoot_valid_(false) +{ +} + +void AxisGoalChecker::initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr/*costmap_ros*/) +{ + plugin_name_ = plugin_name; + auto node = parent.lock(); + logger_ = node->get_logger(); + + along_path_tolerance_ = node->declare_or_get_parameter( + plugin_name + ".along_path_tolerance", 0.25); + cross_track_tolerance_ = node->declare_or_get_parameter( + plugin_name + ".cross_track_tolerance", 0.25); + path_length_tolerance_ = node->declare_or_get_parameter( + plugin_name + ".path_length_tolerance", 1.0); + is_overshoot_valid_ = node->declare_or_get_parameter( + plugin_name + ".is_overshoot_valid", false); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&AxisGoalChecker::dynamicParametersCallback, this, _1)); +} + +void AxisGoalChecker::reset() +{ +} + +bool AxisGoalChecker::isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist &, + const nav_msgs::msg::Path & transformed_global_plan) +{ + // If the local plan length is longer than the tolerance, we skip the check + if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) > + path_length_tolerance_) + { + return false; + } + + // Check if we have at least 2 poses to determine path direction + if (transformed_global_plan.poses.size() >= 2) { + // Use axis-aligned goal checking with path direction + // Find a pose before goal that is sufficiently far from goal + const geometry_msgs::msg::Pose * before_goal_pose_ptr = nullptr; + double dx = 0.0; + double dy = 0.0; + + for (int i = transformed_global_plan.poses.size() - 2; i >= 0; --i) { + const auto & candidate_pose = transformed_global_plan.poses[i].pose; + dx = goal_pose.position.x - candidate_pose.position.x; + dy = goal_pose.position.y - candidate_pose.position.y; + double pose_distance = std::hypot(dx, dy); + + if (pose_distance >= 1e-6) { + before_goal_pose_ptr = &candidate_pose; + break; + } + } + + // If all poses are too close to goal, fall back to simple distance check + if (!before_goal_pose_ptr) { + RCLCPP_DEBUG( + logger_, + "All poses in path are too close to goal, falling back to simple distance check"); + double distance_to_goal = std::hypot( + goal_pose.position.x - query_pose.position.x, + goal_pose.position.y - query_pose.position.y); + double tolerance = std::hypot(along_path_tolerance_, cross_track_tolerance_); + return distance_to_goal < tolerance; + } + + // end of path direction + double end_of_path_yaw = atan2(dy, dx); + + // Check if robot is already at goal (would cause atan2(0,0)) + double robot_to_goal_dx = goal_pose.position.x - query_pose.position.x; + double robot_to_goal_dy = goal_pose.position.y - query_pose.position.y; + double distance_to_goal = std::hypot(robot_to_goal_dx, robot_to_goal_dy); + + if (distance_to_goal < 1e-6) { + return true; // Robot is at goal + } + + double robot_to_goal_yaw = atan2(robot_to_goal_dy, robot_to_goal_dx); + double projection_angle = angles::shortest_angular_distance( + robot_to_goal_yaw, end_of_path_yaw); + double along_path_distance = distance_to_goal * cos(projection_angle); + double cross_track_distance = distance_to_goal * sin(projection_angle); + + if (is_overshoot_valid_) { + return along_path_distance < along_path_tolerance_ && + fabs(cross_track_distance) < cross_track_tolerance_; + } else { + return fabs(along_path_distance) < along_path_tolerance_ && + fabs(cross_track_distance) < cross_track_tolerance_; + } + } else { + // Fallback: path has only 1 point, use simple distance check + RCLCPP_DEBUG( + logger_, + "Path has fewer than 2 poses, falling back to simple distance check"); + double distance_to_goal = std::hypot( + goal_pose.position.x - query_pose.position.x, + goal_pose.position.y - query_pose.position.y); + double tolerance = std::hypot(along_path_tolerance_, cross_track_tolerance_); + return distance_to_goal < tolerance; + } +} + +bool AxisGoalChecker::getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) +{ + double invalid_field = std::numeric_limits::lowest(); + + pose_tolerance.position.x = std::min(along_path_tolerance_, cross_track_tolerance_); + pose_tolerance.position.y = std::min(along_path_tolerance_, cross_track_tolerance_); + pose_tolerance.position.z = invalid_field; + pose_tolerance.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + + vel_tolerance.linear.x = invalid_field; + vel_tolerance.linear.y = invalid_field; + vel_tolerance.linear.z = invalid_field; + + vel_tolerance.angular.x = invalid_field; + vel_tolerance.angular.y = invalid_field; + vel_tolerance.angular.z = invalid_field; + + return true; +} + +rcl_interfaces::msg::SetParametersResult +AxisGoalChecker::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto & parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + if (name.find(plugin_name_ + ".") != 0) { + continue; + } + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".along_path_tolerance") { + along_path_tolerance_ = parameter.as_double(); + } else if (name == plugin_name_ + ".cross_track_tolerance") { + cross_track_tolerance_ = parameter.as_double(); + } else if (name == plugin_name_ + ".path_length_tolerance") { + path_length_tolerance_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".is_overshoot_valid") { + is_overshoot_valid_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::AxisGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index 3b3663f5c96..e8a4c579f3a 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -4,5 +4,8 @@ target_link_libraries(pctest simple_progress_checker pose_progress_checker) nav2_add_gtest(gctest goal_checker.cpp) target_link_libraries(gctest simple_goal_checker stopped_goal_checker position_goal_checker) +nav2_add_gtest(agctest axis_goal_checker.cpp) +target_link_libraries(agctest axis_goal_checker) + nav2_add_gtest(phtest path_handler.cpp) target_link_libraries(phtest feasible_path_handler) diff --git a/nav2_controller/plugins/test/axis_goal_checker.cpp b/nav2_controller/plugins/test/axis_goal_checker.cpp new file mode 100644 index 00000000000..01ee491e768 --- /dev/null +++ b/nav2_controller/plugins/test/axis_goal_checker.cpp @@ -0,0 +1,618 @@ +// Copyright (c) 2025 Dexory +// +// 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 +#include +#include + +#include "gtest/gtest.h" +#include "nav2_controller/plugins/axis_goal_checker.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav_msgs/msg/path.hpp" + +using nav2_controller::AxisGoalChecker; + +class TestLifecycleNode : public nav2::LifecycleNode +{ +public: + explicit TestLifecycleNode(const std::string & name) + : nav2::LifecycleNode(name) + { + } + + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2::CallbackReturn::SUCCESS; + } +}; + +// Helper function to create a path with multiple poses +nav_msgs::msg::Path createPath(const std::vector> & positions) +{ + nav_msgs::msg::Path path; + for (const auto & pos : positions) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = pos.first; + pose.pose.position.y = pos.second; + pose.pose.position.z = 0.0; + pose.pose.orientation.w = 1.0; + path.poses.push_back(pose); + } + return path; +} + +TEST(AxisGoalChecker, reset) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + agc.reset(); + EXPECT_TRUE(true); +} + +TEST(AxisGoalChecker, initialize_and_tolerances) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + geometry_msgs::msg::Pose pose_tol; + geometry_msgs::msg::Twist vel_tol; + + // Test tolerance API + EXPECT_TRUE(agc.getTolerances(pose_tol, vel_tol)); + // Default tolerances should be 0.25 + EXPECT_EQ(pose_tol.position.x, 0.25); + EXPECT_EQ(pose_tol.position.y, 0.25); +} + +TEST(AxisGoalChecker, dynamic_parameters) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + // Test dynamic parameters + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.along_path_tolerance", 0.5), + rclcpp::Parameter("test.cross_track_tolerance", 0.3), + rclcpp::Parameter("test.path_length_tolerance", 2.0), + rclcpp::Parameter("test.is_overshoot_valid", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("test.along_path_tolerance").as_double(), 0.5); + EXPECT_EQ(node->get_parameter("test.cross_track_tolerance").as_double(), 0.3); + EXPECT_EQ(node->get_parameter("test.path_length_tolerance").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.is_overshoot_valid").as_bool(), true); + + // Test that tolerances are updated + geometry_msgs::msg::Pose pose_tol; + geometry_msgs::msg::Twist vel_tol; + EXPECT_TRUE(agc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(pose_tol.position.x, 0.3); // min of along_path and cross_track + EXPECT_EQ(pose_tol.position.y, 0.3); +} + +TEST(AxisGoalChecker, single_point_path) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + // Create a path with only one point (goal) + nav_msgs::msg::Path path = createPath({{0.0, 0.0}}); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 0.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Robot at goal + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, path)); + + // Robot within tolerance + query_pose.position.x = 0.2; + query_pose.position.y = 0.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, path)); + + // Robot outside combined tolerance (hypot(0.25, 0.25) ≈ 0.354) + query_pose.position.x = 0.36; + query_pose.position.y = 0.0; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, path)); +} + +TEST(AxisGoalChecker, straight_path_along_x) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a short path (within path_length_tolerance) for checking + nav_msgs::msg::Path short_path = createPath({{1.5, 0.0}, {2.0, 0.0}}); + + // Robot at goal + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot slightly before goal (within along_path_tolerance) + query_pose.position.x = 1.8; + query_pose.position.y = 0.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot too far before goal (outside along_path_tolerance) + query_pose.position.x = 1.74; + query_pose.position.y = 0.0; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with small cross-track error (within cross_track_tolerance) + query_pose.position.x = 2.0; + query_pose.position.y = 0.2; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with large cross-track error (outside cross_track_tolerance) + query_pose.position.x = 2.0; + query_pose.position.y = 0.3; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot slightly past goal (should fail without is_overshoot_valid) + query_pose.position.x = 2.26; + query_pose.position.y = 0.0; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, straight_path_along_y) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 0.0; + goal_pose.position.y = 2.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a short path (within path_length_tolerance) for checking + nav_msgs::msg::Path short_path = createPath({{0.0, 1.5}, {0.0, 2.0}}); + + // Robot at goal + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot slightly before goal (within along_path_tolerance) + query_pose.position.x = 0.0; + query_pose.position.y = 1.8; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with cross-track error (within tolerance) + query_pose.position.x = 0.2; + query_pose.position.y = 2.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with large cross-track error (outside tolerance) + query_pose.position.x = 0.3; + query_pose.position.y = 2.0; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, diagonal_path) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 2.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a short diagonal path (within path_length_tolerance) for checking + nav_msgs::msg::Path short_path = createPath({{1.65, 1.65}, {2.0, 2.0}}); + + // Robot at goal + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot slightly before goal along the diagonal (within along_path_tolerance) + // Moving 0.2 along the path at 45 degrees: dx = dy = 0.2 * cos(45) ≈ 0.14 + double delta = 0.14; + query_pose.position.x = 2.0 - delta; + query_pose.position.y = 2.0 - delta; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with cross-track error perpendicular to path + // For a 45-degree path, perpendicular offset at +45 degrees to the right + query_pose.position.x = 2.0 + 0.1; + query_pose.position.y = 2.0 - 0.1; // perpendicular to the path + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with large cross-track error + query_pose.position.x = 2.0 + 0.2; + query_pose.position.y = 2.0 - 0.2; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, overshoot_valid) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + // Enable overshoot + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.is_overshoot_valid", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a short path along x-axis (within path_length_tolerance) for checking + nav_msgs::msg::Path short_path = createPath({{1.5, 0.0}, {2.0, 0.0}}); + + // Robot slightly past goal (should pass with is_overshoot_valid=true) + query_pose.position.x = 2.15; + query_pose.position.y = 0.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot far past goal (should still pass with overshoot valid - any overshoot allowed) + query_pose.position.x = 10.0; + query_pose.position.y = 0.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot before goal (should still pass) + query_pose.position.x = 1.8; + query_pose.position.y = 0.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, path_length_tolerance) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + // Create a long path that exceeds path_length_tolerance (default 1.0) + nav_msgs::msg::Path long_path = createPath({ + {0.0, 0.0}, {0.5, 0.0}, {1.0, 0.0}, {1.5, 0.0}, {2.0, 0.0} + }); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + query_pose.position.x = 1.9; + query_pose.position.y = 0.0; + query_pose.position.z = 0.0; + query_pose.orientation.w = 1.0; + + geometry_msgs::msg::Twist velocity; + + // Path length is 2.0, exceeds tolerance of 1.0, so should return false + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, long_path)); + + // Create a short path within tolerance + nav_msgs::msg::Path short_path = createPath({{1.5, 0.0}, {2.0, 0.0}}); + // Path length is 0.5, within tolerance of 1.0, so should check goal + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, combined_errors) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a short path along x-axis (within path_length_tolerance) for checking + nav_msgs::msg::Path short_path = createPath({{1.5, 0.0}, {2.0, 0.0}}); + + // Robot with both along-path and cross-track errors, both within tolerance + query_pose.position.x = 1.9; // 0.1 along-path error + query_pose.position.y = 0.15; // 0.15 cross-track error + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with along-path error within tolerance but cross-track error outside + query_pose.position.x = 1.9; + query_pose.position.y = 0.3; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with cross-track error within tolerance but along-path error outside + query_pose.position.x = 1.7; + query_pose.position.y = 0.15; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, angled_approach) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + // Create a path with a 30-degree angle + double angle = M_PI / 6.0; // 30 degrees + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0 * cos(angle); + goal_pose.position.y = 2.0 * sin(angle); + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a short path (within path_length_tolerance) for checking + nav_msgs::msg::Path short_path = createPath({ + {1.65 * cos(angle), 1.65 * sin(angle)}, + {2.0 * cos(angle), 2.0 * sin(angle)} + }); + + // Robot at goal + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot slightly before goal along the path direction + double delta = 0.15; + query_pose.position.x = goal_pose.position.x - delta * cos(angle); + query_pose.position.y = goal_pose.position.y - delta * sin(angle); + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot with cross-track offset (perpendicular to path) + double cross_offset = 0.15; + query_pose.position.x = goal_pose.position.x + cross_offset * cos(angle + M_PI / 2.0); + query_pose.position.y = goal_pose.position.y + cross_offset * sin(angle + M_PI / 2.0); + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, identical_consecutive_poses) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + // Create a path where the last two poses are identical + nav_msgs::msg::Path path_with_duplicates = createPath({{1.5, 0.0}, {2.0, 0.0}, {2.0, 0.0}}); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Robot within tolerance should succeed (falls back to simple distance check) + query_pose.position.x = 1.85; + query_pose.position.y = 0.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, path_with_duplicates)); + + // Robot at goal + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, path_with_duplicates)); + + // Robot outside tolerance should fail + query_pose.position.x = 1.7; + query_pose.position.y = 0.0; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, path_with_duplicates)); +} + +TEST(AxisGoalChecker, robot_at_goal_position) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a normal path + nav_msgs::msg::Path short_path = createPath({{1.5, 0.0}, {2.0, 0.0}}); + + // Robot exactly at goal position + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); + + // Robot extremely close to goal (within numerical precision threshold) + query_pose.position.x = 2.0 + 1e-7; + query_pose.position.y = 0.0 + 1e-7; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, short_path)); +} + +TEST(AxisGoalChecker, multiple_consecutive_poses_too_close_to_goal) +{ + auto node = std::make_shared("axis_goal_checker_test"); + AxisGoalChecker agc; + auto costmap = std::make_shared("test_costmap"); + + agc.initialize(node, "test", costmap); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position.x = 2.0; + goal_pose.position.y = 0.0; + goal_pose.position.z = 0.0; + goal_pose.orientation.w = 1.0; + + geometry_msgs::msg::Pose query_pose; + geometry_msgs::msg::Twist velocity; + + // Create a path where multiple consecutive poses at the end are extremely close to goal + // (within 1e-6 distance threshold), but there's a valid pose further back + // Keep path short to stay within path_length_tolerance + nav_msgs::msg::Path path_with_close_poses = createPath({ + {1.8, 0.0}, // Valid pose before goal + {2.0 - 0.001, 0.0}, // Valid pose 1mm before goal (> 1e-6 threshold) + {2.0 + 1e-7, 0.0}, // Too close to goal + {2.0 + 5e-8, 1e-8}, // Too close to goal + {2.0, 0.0} // Goal position (identical) + }); + + // Robot within tolerance - should use the valid pose at (2.0-0.001, 0.0) + // to determine path direction + query_pose.position.x = 1.85; + query_pose.position.y = 0.0; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, path_with_close_poses)); + + // Robot at goal + query_pose = goal_pose; + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, path_with_close_poses)); + + // Robot outside tolerance should fail + query_pose.position.x = 1.7; + query_pose.position.y = 0.0; + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, path_with_close_poses)); + + // Create a path where ALL poses are too close to goal (should fall back to distance check) + nav_msgs::msg::Path path_all_close = createPath({ + {2.0 + 1e-7, 0.0}, + {2.0 + 5e-8, 1e-8}, + {2.0, 0.0} + }); + + // Robot within combined tolerance should succeed (fallback to distance check) + query_pose.position.x = 2.0 + 0.2; + query_pose.position.y = 0.15; + double combined_tolerance = std::hypot(0.25, 0.25); + double distance = std::hypot(0.2, 0.15); + EXPECT_LT(distance, combined_tolerance); // Verify we're within tolerance + EXPECT_TRUE(agc.isGoalReached(query_pose, goal_pose, velocity, path_all_close)); + + // Robot outside combined tolerance should fail + query_pose.position.x = 2.0 + 0.3; + query_pose.position.y = 0.3; + distance = std::hypot(0.3, 0.3); + EXPECT_GT(distance, combined_tolerance); // Verify we're outside tolerance + EXPECT_FALSE(agc.isGoalReached(query_pose, goal_pose, velocity, path_all_close)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +}