From 7b3dac8be775a8941cae15592c9de67c94913240 Mon Sep 17 00:00:00 2001 From: StetroF <120172218+StetroF@users.noreply.github.com> Date: Wed, 15 May 2024 23:48:48 +0800 Subject: [PATCH 1/5] Update path_longer_on_approach.cpp (#4344) Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> --- .../plugins/decorator/path_longer_on_approach.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index eb401f6762a..1b3f8abd564 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated( { return new_path != old_path && old_path.poses.size() != 0 && new_path.poses.size() != 0 && - old_path.poses.back() == new_path.poses.back(); + old_path.poses.back().pose == new_path.poses.back().pose; } bool PathLongerOnApproach::isRobotInGoalProximity( From 5acdb4f6bfbf33ff00308fe76f56aad62e1e8b5c Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 15 May 2024 16:51:04 +0100 Subject: [PATCH 2/5] [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- .../lifecycle_manager_client.hpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index c5f2be8bb62..076e848902c 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -84,23 +84,6 @@ class LifecycleManagerClient */ SystemStatus is_active(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - // A couple convenience methods to facilitate scripting tests - /** - * @brief Set initial pose with covariance - * @param x X position - * @param y Y position - * @param theta orientation - */ - void set_initial_pose(double x, double y, double theta); - /** - * @brief Send goal pose to NavigationToPose action server - * @param x X position - * @param y Y position - * @param theta orientation - * @return true or false - */ - bool navigate_to_pose(double x, double y, double theta); - protected: using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes; From bf291d704dbb1c2572575dd0620b12f816407233 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 16 May 2024 14:14:28 -0700 Subject: [PATCH 3/5] Remove flaky spin test that needs to be rewritten (#4348) * remove spin test that is failing reliably * cont. --- .../spin/test_spin_behavior_node.cpp | 66 +++++++++---------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index 6838e012219..cf0f97021db 100644 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -58,27 +58,27 @@ class SpinBehaviorTestFixture SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr; -TEST_P(SpinBehaviorTestFixture, testSpinRecovery) -{ - float target_yaw = std::get<0>(GetParam()); - float tolerance = std::get<1>(GetParam()); - - bool success = false; - int num_tries = 3; - for (int i = 0; i != num_tries; i++) { - success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance); - if (success) { - break; - } - } - if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) { - // if this variable is set, make a fake costmap - // in the fake spin test, we expect a collision for angles > M_PI_2 - EXPECT_EQ(false, success); - } else { - EXPECT_EQ(true, success); - } -} +// TEST_P(SpinBehaviorTestFixture, testSpinRecovery) +// { +// float target_yaw = std::get<0>(GetParam()); +// float tolerance = std::get<1>(GetParam()); + +// bool success = false; +// int num_tries = 3; +// for (int i = 0; i != num_tries; i++) { +// success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance); +// if (success) { +// break; +// } +// } +// if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) { +// // if this variable is set, make a fake costmap +// // in the fake spin test, we expect a collision for angles > M_PI_2 +// EXPECT_EQ(false, success); +// } else { +// EXPECT_EQ(true, success); +// } +// } TEST_F(SpinBehaviorTestFixture, testSpinPreemption) { @@ -114,18 +114,18 @@ TEST_F(SpinBehaviorTestFixture, testSpinCancel) EXPECT_EQ(false, success); } -INSTANTIATE_TEST_SUITE_P( - SpinRecoveryTests, - SpinBehaviorTestFixture, - ::testing::Values( - std::make_tuple(-M_PIf32 / 6.0, 0.1), - // std::make_tuple(M_PI_4f32, 0.1), - // std::make_tuple(-M_PI_2f32, 0.1), - std::make_tuple(M_PIf32, 0.1), - std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), - std::make_tuple(-2.0 * M_PIf32, 0.1), - std::make_tuple(4.0 * M_PIf32, 0.15)), - testNameGenerator); +// INSTANTIATE_TEST_SUITE_P( +// SpinRecoveryTests, +// SpinBehaviorTestFixture, +// ::testing::Values( +// std::make_tuple(-M_PIf32 / 6.0, 0.1), +// // std::make_tuple(M_PI_4f32, 0.1), +// // std::make_tuple(-M_PI_2f32, 0.1), +// std::make_tuple(M_PIf32, 0.1), +// std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), +// std::make_tuple(-2.0 * M_PIf32, 0.1), +// std::make_tuple(4.0 * M_PIf32, 0.15)), +// testNameGenerator); int main(int argc, char ** argv) { From df2d686548ca7ba97772c681907a5b90ee01341f Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Tue, 21 May 2024 19:09:17 +0200 Subject: [PATCH 4/5] Move projectState after getPointsInside (#4356) * Modify test to check fix Signed-off-by: Brice * Add static polygon check before simulation Signed-off-by: Brice --------- Signed-off-by: Brice --- nav2_collision_monitor/src/polygon.cpp | 7 ++++++- .../test/collision_monitor_node_test.cpp | 3 ++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 08910c91458..523b446b25a 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -238,7 +238,12 @@ double Polygon::getCollisionTime( Velocity vel = velocity; // Array of points transformed to the frame concerned with pose on each simulation step - std::vector points_transformed; + std::vector points_transformed = collision_points; + + // Check static polygon + if (getPointsInside(points_transformed) >= min_points_) { + return 0.0; + } // Robot movement simulation for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) { diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 69f8ac21e06..81884a6a455 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -991,7 +991,8 @@ TEST_F(Tester, testProcessApproach) // 3. Obstacle is inside robot footprint publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); - publishCmdVel(0.5, 0.2, 0.0); + // Publish impossible cmd_vel to ensure robot footprint is checked + publishCmdVel(1000000000.0, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); From 4995008f2d40194c5a2443533c1770f6f9d4a46a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 23 May 2024 08:21:02 -0700 Subject: [PATCH 5/5] adding final pose in analytic expansion to check (#4353) --- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index f2667d3ba8e..455a6e2a217 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -210,7 +210,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansioninterpolate(from(), to(), i / num_intervals, s()); reals = s.reals(); // Make sure in range [0, 2PI)