diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 30b90f4f4ab..d49b94c2873 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -114,7 +114,7 @@ bool SimpleGoalChecker::isGoalReached( double dyaw = angles::shortest_angular_distance( tf2::getYaw(query_pose.orientation), tf2::getYaw(goal_pose.orientation)); - return fabs(dyaw) < yaw_goal_tolerance_; + return fabs(dyaw) <= yaw_goal_tolerance_; } bool SimpleGoalChecker::getTolerances( diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 303921bb17e..5faba1bdc79 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -40,6 +40,7 @@ #include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "eigen3/Eigen/Geometry" using nav2_controller::SimpleGoalChecker; using nav2_controller::StoppedGoalChecker; @@ -237,6 +238,108 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(pose_tol.position.y, 200.0); } +TEST(StoppedGoalChecker, is_reached) +{ + 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 goal_pose; + geometry_msgs::msg::Twist velocity; + geometry_msgs::msg::Pose current_pose; + + // Current linear x position is tolerance away from goal + current_pose.position.x = 0.25; + velocity.linear.x = 0.25; + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear x speed exceeds tolerance + velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear x position is further than tolerance away from goal + current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); + velocity.linear.x = 0.25; + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + current_pose.position.x = 0.0; + velocity.linear.x = 0.0; + + // Current linear position is tolerance away from goal + current_pose.position.x = 0.25 / std::sqrt(2); + current_pose.position.y = 0.25 / std::sqrt(2); + velocity.linear.x = 0.25 / std::sqrt(2); + velocity.linear.y = 0.25 / std::sqrt(2); + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear speed exceeds tolerance + velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear position is further than tolerance away from goal + current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + velocity.linear.x = 0.25 / std::sqrt(2); + velocity.linear.y = 0.25 / std::sqrt(2); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + current_pose.position.x = 0.0; + velocity.linear.x = 0.0; + + + // Current angular position is tolerance away from goal + auto quat = + (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); + // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw + auto quat_epsilon = + (Eigen::AngleAxisd::Identity() * + Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); + + current_pose.orientation.z = quat[2]; + current_pose.orientation.w = quat[3]; + velocity.angular.z = 0.25; + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current angular speed exceeds tolerance + velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current angular position is further than tolerance away from goal + current_pose.orientation.z = quat_epsilon[2]; + current_pose.orientation.w = quat_epsilon[3]; + velocity.angular.z = 0.25; + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp index 228f51fe9c9..4c5e0b28a90 100644 --- a/nav2_controller/plugins/test/progress_checker.cpp +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -84,12 +84,11 @@ class TestLifecycleNode : public nav2_util::LifecycleNode } }; -void checkMacro( +bool checkMacro( nav2_core::ProgressChecker & pc, double x0, double y0, double theta0, double x1, double y1, double theta1, - int delay, - bool expected_result) + int delay) { pc.reset(); geometry_msgs::msg::PoseStamped pose0, pose1; @@ -101,11 +100,7 @@ void checkMacro( pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); EXPECT_TRUE(pc.check(pose0)); rclcpp::sleep_for(std::chrono::milliseconds(delay)); - if (expected_result) { - EXPECT_TRUE(pc.check(pose1)); - } else { - EXPECT_FALSE(pc.check(pose1)); - } + return pc.check(pose1); } TEST(SimpleProgressChecker, progress_checker_reset) @@ -125,7 +120,7 @@ TEST(SimpleProgressChecker, unit_tests) SimpleProgressChecker pc; pc.initialize(x, "nav2_controller"); - double time_allowance = 0.5; + double time_allowance = 0.1; int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); @@ -147,23 +142,83 @@ TEST(SimpleProgressChecker, unit_tests) // BELOW time allowance (set to time_allowance) // no movement - checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); - checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); - checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms)); // ABOVE time allowance (set to time_allowance) // no movement - checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); - checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); - checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms)); +} + +TEST(SimpleProgressChecker, required_movement_radius) { + auto lifecycle_node = std::make_shared("progress_checker"); + + SimpleProgressChecker progress_checker; + progress_checker.initialize(lifecycle_node, "nav2_controller"); + + auto parameters_client = std::make_shared( + lifecycle_node->get_node_base_interface(), lifecycle_node->get_node_topics_interface(), + lifecycle_node->get_node_graph_interface(), + lifecycle_node->get_node_services_interface()); + + constexpr double radius = 0.25; + constexpr double time_allowance = 0.05; + constexpr double twice_time_allowance_ms = time_allowance * 2.0 * 1000; + auto results = parameters_client->set_parameters_atomically( + {rclcpp::Parameter("nav2_controller.required_movement_radius", radius), + rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)}); + rclcpp::spin_until_future_complete( + lifecycle_node->get_node_base_interface(), + results); + + EXPECT_EQ( + lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(), + radius); + + // ABOVE time allowance (set to time_allowance) + // no movement + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); + // translation at required_movement_radius one axis + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms)); + constexpr auto axis = radius / std::sqrt(2); + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms)); + // translation at required_movement_radius both axes + + // translation above required_movement_radius one axis + constexpr auto above = radius + std::numeric_limits::epsilon(); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, above, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, above, 0, twice_time_allowance_ms)); + // translation at required_movement_radius both axes + constexpr auto above_axis = axis + std::numeric_limits::epsilon(); + EXPECT_TRUE( + checkMacro(progress_checker, 0, 0, 0, above_axis, above_axis, 0, twice_time_allowance_ms)); + + // Edge case, negative radius always true + results = parameters_client->set_parameters_atomically( + {rclcpp::Parameter("nav2_controller.required_movement_radius", -radius)}); + rclcpp::spin_until_future_complete( + lifecycle_node->get_node_base_interface(), + results); + EXPECT_EQ( + lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(), + -radius); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); + // translation at required_movement_radius one axis + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms)); } TEST(PoseProgressChecker, pose_progress_checker_reset) @@ -183,7 +238,7 @@ TEST(PoseProgressChecker, unit_tests) PoseProgressChecker rpc; rpc.initialize(x, "nav2_controller"); - double time_allowance = 0.5; + double time_allowance = 0.1; int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); @@ -205,35 +260,35 @@ TEST(PoseProgressChecker, unit_tests) // BELOW time allowance (set to time_allowance) // no movement - checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms)); // rotation below required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms)); // rotation above required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms)); // ABOVE time allowance (set to time_allowance) // no movement - checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); - checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms)); // rotation below required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false); - checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms)); // rotation above required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms)); } int main(int argc, char ** argv)