Skip to content

Commit e7335af

Browse files
RasmusLarRasmusLar
committed
Change to goal checker orientation for yaw angle
- Fixed discrepancy in goal checker orientation, which was checking for < tolerance instead of <= tolerance, as all the other limit checks are. - Reduced tolerance time for the progress checker unit tests to 0.1 seconds, to reduce test runtime from ~17 to ~7 seconds.
1 parent abcd8fe commit e7335af

File tree

3 files changed

+197
-39
lines changed

3 files changed

+197
-39
lines changed

‎nav2_controller/plugins/simple_goal_checker.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,7 @@ bool SimpleGoalChecker::isGoalReached(
114114
double dyaw = angles::shortest_angular_distance(
115115
tf2::getYaw(query_pose.orientation),
116116
tf2::getYaw(goal_pose.orientation));
117-
return fabs(dyaw) < yaw_goal_tolerance_;
117+
return fabs(dyaw) <= yaw_goal_tolerance_;
118118
}
119119

120120
bool SimpleGoalChecker::getTolerances(

‎nav2_controller/plugins/test/goal_checker.cpp‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
#include "nav2_controller/plugins/stopped_goal_checker.hpp"
4141
#include "nav_2d_utils/conversions.hpp"
4242
#include "nav2_util/lifecycle_node.hpp"
43+
#include "eigen3/Eigen/Geometry"
4344

4445
using nav2_controller::SimpleGoalChecker;
4546
using nav2_controller::StoppedGoalChecker;
@@ -237,6 +238,108 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
237238
EXPECT_EQ(pose_tol.position.y, 200.0);
238239
}
239240

241+
TEST(StoppedGoalChecker, is_reached)
242+
{
243+
auto x = std::make_shared<TestLifecycleNode>("goal_checker");
244+
245+
SimpleGoalChecker gc;
246+
StoppedGoalChecker sgc;
247+
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
248+
249+
sgc.initialize(x, "test", costmap);
250+
gc.initialize(x, "test2", costmap);
251+
geometry_msgs::msg::Pose goal_pose;
252+
geometry_msgs::msg::Twist velocity;
253+
geometry_msgs::msg::Pose current_pose;
254+
255+
// Current linear x position is tolerance away from goal
256+
current_pose.position.x = 0.25;
257+
velocity.linear.x = 0.25;
258+
EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity));
259+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
260+
sgc.reset();
261+
gc.reset();
262+
263+
// Current linear x speed exceeds tolerance
264+
velocity.linear.x = 0.25 + std::numeric_limits<double>::epsilon();
265+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
266+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
267+
sgc.reset();
268+
gc.reset();
269+
270+
// Current linear x position is further than tolerance away from goal
271+
current_pose.position.x = 0.25 + std::numeric_limits<double>::epsilon();
272+
velocity.linear.x = 0.25;
273+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
274+
EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity));
275+
sgc.reset();
276+
gc.reset();
277+
current_pose.position.x = 0.0;
278+
velocity.linear.x = 0.0;
279+
280+
// Current linear position is tolerance away from goal
281+
current_pose.position.x = 0.25 / std::sqrt(2);
282+
current_pose.position.y = 0.25 / std::sqrt(2);
283+
velocity.linear.x = 0.25 / std::sqrt(2);
284+
velocity.linear.y = 0.25 / std::sqrt(2);
285+
EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity));
286+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
287+
sgc.reset();
288+
gc.reset();
289+
290+
// Current linear speed exceeds tolerance
291+
velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
292+
velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
293+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
294+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
295+
sgc.reset();
296+
gc.reset();
297+
298+
// Current linear position is further than tolerance away from goal
299+
current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
300+
current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
301+
velocity.linear.x = 0.25 / std::sqrt(2);
302+
velocity.linear.y = 0.25 / std::sqrt(2);
303+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
304+
EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity));
305+
sgc.reset();
306+
gc.reset();
307+
308+
current_pose.position.x = 0.0;
309+
velocity.linear.x = 0.0;
310+
311+
312+
// Current angular position is tolerance away from goal
313+
auto quat =
314+
(Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs();
315+
// epsilon for orientation is a lot bigger than double limit, probably from TF getYaw
316+
auto quat_epsilon =
317+
(Eigen::AngleAxisd::Identity() *
318+
Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs();
319+
320+
current_pose.orientation.z = quat[2];
321+
current_pose.orientation.w = quat[3];
322+
velocity.angular.z = 0.25;
323+
EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity));
324+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
325+
sgc.reset();
326+
gc.reset();
327+
328+
// Current angular speed exceeds tolerance
329+
velocity.angular.z = 0.25 + std::numeric_limits<double>::epsilon();
330+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
331+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
332+
sgc.reset();
333+
gc.reset();
334+
335+
// Current angular position is further than tolerance away from goal
336+
current_pose.orientation.z = quat_epsilon[2];
337+
current_pose.orientation.w = quat_epsilon[3];
338+
velocity.angular.z = 0.25;
339+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
340+
EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity));
341+
}
342+
240343
int main(int argc, char ** argv)
241344
{
242345
::testing::InitGoogleTest(&argc, argv);

‎nav2_controller/plugins/test/progress_checker.cpp‎

Lines changed: 93 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -84,12 +84,11 @@ class TestLifecycleNode : public nav2_util::LifecycleNode
8484
}
8585
};
8686

87-
void checkMacro(
87+
bool checkMacro(
8888
nav2_core::ProgressChecker & pc,
8989
double x0, double y0, double theta0,
9090
double x1, double y1, double theta1,
91-
int delay,
92-
bool expected_result)
91+
int delay)
9392
{
9493
pc.reset();
9594
geometry_msgs::msg::PoseStamped pose0, pose1;
@@ -101,11 +100,7 @@ void checkMacro(
101100
pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1);
102101
EXPECT_TRUE(pc.check(pose0));
103102
rclcpp::sleep_for(std::chrono::milliseconds(delay));
104-
if (expected_result) {
105-
EXPECT_TRUE(pc.check(pose1));
106-
} else {
107-
EXPECT_FALSE(pc.check(pose1));
108-
}
103+
return pc.check(pose1);
109104
}
110105

111106
TEST(SimpleProgressChecker, progress_checker_reset)
@@ -125,7 +120,7 @@ TEST(SimpleProgressChecker, unit_tests)
125120
SimpleProgressChecker pc;
126121
pc.initialize(x, "nav2_controller");
127122

128-
double time_allowance = 0.5;
123+
double time_allowance = 0.1;
129124
int half_time_allowance_ms = static_cast<int>(time_allowance * 0.5 * 1000);
130125
int twice_time_allowance_ms = static_cast<int>(time_allowance * 2.0 * 1000);
131126

@@ -147,23 +142,83 @@ TEST(SimpleProgressChecker, unit_tests)
147142

148143
// BELOW time allowance (set to time_allowance)
149144
// no movement
150-
checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
145+
EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms));
151146
// translation below required_movement_radius (default 0.5)
152-
checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
153-
checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
147+
EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms));
148+
EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms));
154149
// translation above required_movement_radius (default 0.5)
155-
checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
156-
checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
150+
EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms));
151+
EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms));
157152

158153
// ABOVE time allowance (set to time_allowance)
159154
// no movement
160-
checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
155+
EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms));
161156
// translation below required_movement_radius (default 0.5)
162-
checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
163-
checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
157+
EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms));
158+
EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms));
164159
// translation above required_movement_radius (default 0.5)
165-
checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
166-
checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
160+
EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms));
161+
EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms));
162+
}
163+
164+
TEST(SimpleProgressChecker, required_movement_radius) {
165+
auto lifecycle_node = std::make_shared<TestLifecycleNode>("progress_checker");
166+
167+
SimpleProgressChecker progress_checker;
168+
progress_checker.initialize(lifecycle_node, "nav2_controller");
169+
170+
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
171+
lifecycle_node->get_node_base_interface(), lifecycle_node->get_node_topics_interface(),
172+
lifecycle_node->get_node_graph_interface(),
173+
lifecycle_node->get_node_services_interface());
174+
175+
constexpr double radius = 0.25;
176+
constexpr double time_allowance = 0.05;
177+
constexpr double twice_time_allowance_ms = time_allowance * 2.0 * 1000;
178+
auto results = parameters_client->set_parameters_atomically(
179+
{rclcpp::Parameter("nav2_controller.required_movement_radius", radius),
180+
rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)});
181+
rclcpp::spin_until_future_complete(
182+
lifecycle_node->get_node_base_interface(),
183+
results);
184+
185+
EXPECT_EQ(
186+
lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(),
187+
radius);
188+
189+
// ABOVE time allowance (set to time_allowance)
190+
// no movement
191+
EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms));
192+
// translation at required_movement_radius one axis
193+
EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms));
194+
EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms));
195+
constexpr auto axis = radius / std::sqrt(2);
196+
EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms));
197+
// translation at required_movement_radius both axes
198+
199+
// translation above required_movement_radius one axis
200+
constexpr auto above = radius + std::numeric_limits<double>::epsilon();
201+
EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, above, 0, 0, twice_time_allowance_ms));
202+
EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, above, 0, twice_time_allowance_ms));
203+
// translation at required_movement_radius both axes
204+
constexpr auto above_axis = axis + std::numeric_limits<double>::epsilon();
205+
EXPECT_TRUE(
206+
checkMacro(progress_checker, 0, 0, 0, above_axis, above_axis, 0, twice_time_allowance_ms));
207+
208+
// Edge case, negative radius always true
209+
results = parameters_client->set_parameters_atomically(
210+
{rclcpp::Parameter("nav2_controller.required_movement_radius", -radius)});
211+
rclcpp::spin_until_future_complete(
212+
lifecycle_node->get_node_base_interface(),
213+
results);
214+
EXPECT_EQ(
215+
lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(),
216+
-radius);
217+
EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms));
218+
// translation at required_movement_radius one axis
219+
EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms));
220+
EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms));
221+
EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms));
167222
}
168223

169224
TEST(PoseProgressChecker, pose_progress_checker_reset)
@@ -183,7 +238,7 @@ TEST(PoseProgressChecker, unit_tests)
183238
PoseProgressChecker rpc;
184239
rpc.initialize(x, "nav2_controller");
185240

186-
double time_allowance = 0.5;
241+
double time_allowance = 0.1;
187242
int half_time_allowance_ms = static_cast<int>(time_allowance * 0.5 * 1000);
188243
int twice_time_allowance_ms = static_cast<int>(time_allowance * 2.0 * 1000);
189244

@@ -205,35 +260,35 @@ TEST(PoseProgressChecker, unit_tests)
205260

206261
// BELOW time allowance (set to time_allowance)
207262
// no movement
208-
checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
263+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms));
209264
// translation below required_movement_radius (default 0.5)
210-
checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
211-
checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
265+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms));
266+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms));
212267
// rotation below required_movement_angle (default 0.5)
213-
checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true);
214-
checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true);
268+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms));
269+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms));
215270
// translation above required_movement_radius (default 0.5)
216-
checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
217-
checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
271+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms));
272+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms));
218273
// rotation above required_movement_angle (default 0.5)
219-
checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true);
220-
checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true);
274+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms));
275+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms));
221276

222277
// ABOVE time allowance (set to time_allowance)
223278
// no movement
224-
checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
279+
EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms));
225280
// translation below required_movement_radius (default 0.5)
226-
checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
227-
checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
281+
EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms));
282+
EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms));
228283
// rotation below required_movement_angle (default 0.5)
229-
checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false);
230-
checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false);
284+
EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms));
285+
EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms));
231286
// translation above required_movement_radius (default 0.5)
232-
checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
233-
checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
287+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms));
288+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms));
234289
// rotation above required_movement_angle (default 0.5)
235-
checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true);
236-
checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true);
290+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms));
291+
EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms));
237292
}
238293

239294
int main(int argc, char ** argv)

0 commit comments

Comments
 (0)