From b60a955501d64e67c60d46cf75bf9b1738930218 Mon Sep 17 00:00:00 2001 From: Tanja Baumann Date: Thu, 7 Nov 2019 14:21:12 +0100 Subject: [PATCH] ColPrev: No direction change if no obstacle (#13398) * only change direction if in other bin --- .../CollisionPrevention/CollisionPrevention.cpp | 15 ++++++++++----- .../CollisionPreventionTest.cpp | 4 ++-- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 4dcf635f06..862bde93c5 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -289,6 +289,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); const int sp_index_original = setpoint_index; float best_cost = 9999.f; + int new_sp_index = setpoint_index; for (int i = sp_index_original - guidance_bins; i <= sp_index_original + guidance_bins; i++) { @@ -314,13 +315,17 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) { best_cost = bin_cost; - - float angle = math::radians((float)bin * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); - setpoint_dir = {cosf(angle), sinf(angle)}; - setpoint_index = bin; + new_sp_index = bin; } } + + //only change setpoint direction if it was moved to a different bin + if (new_sp_index != setpoint_index) { + float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + setpoint_dir = {cosf(angle), sinf(angle)}; + setpoint_index = new_sp_index; + } } float diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp index 3ab0d93487..a1a0e7b96c 100644 --- a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -421,8 +421,8 @@ TEST_F(CollisionPreventionTest, outsideFOV) matrix::Vector2f setpoint_dir = modified_setpoint / setpoint_length; float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)); float sp_angle_deg = math::degrees(matrix::wrap_2pi(sp_angle_body_frame)); - EXPECT_GT(sp_angle_deg, 45.f); - EXPECT_LT(sp_angle_deg, 225.f); + EXPECT_GE(sp_angle_deg, 45.f); + EXPECT_LE(sp_angle_deg, 225.f); } }