From dfb334d82f4bacf3dd1298a5c2d4340ee10712f3 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] 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 8c3ef81a2a..3bd558396e 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -239,7 +239,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 69f8ac21e0..81884a6a45 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);