Skip to content

Commit

Permalink
test(obstacle_velocity_limiter): added test cases for particle model …
Browse files Browse the repository at this point in the history 
…and approximation method (#5342)

Co-authored-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@bupeshr @maxime-clem
bupeshr and maxime-clem authored Nov 13, 2023
1 parent a0cd7ad commit 8f07678
Showing 1 changed file with 212 additions and 3 deletions.
215 changes: 212 additions & 3 deletions planning/obstacle_velocity_limiter/test/test_collision_distance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ const auto point_in_polygon = [](const auto x, const auto y, const auto & polygo
}) != polygon.outer().end();
};

TEST(TestCollisionDistance, distanceToClosestCollision)
TEST(TestCollisionDistance, distanceToClosestCollisionParticleModel)
{
using obstacle_velocity_limiter::CollisionChecker;
using obstacle_velocity_limiter::distanceToClosestCollision;
Expand Down Expand Up @@ -76,11 +76,11 @@ TEST(TestCollisionDistance, distanceToClosestCollision)
ASSERT_TRUE(result.has_value());
EXPECT_DOUBLE_EQ(*result, 3.0);

obstacles.points.emplace_back(2.5, -0.75);
obstacles.points.emplace_back(2.75, -0.75);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_DOUBLE_EQ(*result, 2.5);
EXPECT_DOUBLE_EQ(*result, 2.75);

// Change vector and footprint
vector = linestring_t{{0.0, 0.0}, {5.0, 5.0}};
Expand All @@ -95,6 +95,7 @@ TEST(TestCollisionDistance, distanceToClosestCollision)
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(4.0, 4.0);

result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
Expand Down Expand Up @@ -151,6 +152,214 @@ TEST(TestCollisionDistance, distanceToClosestCollision)
EXPECT_NEAR(*result, 2.121, 1e-3);
}

TEST(TestCollisionDistance, distanceToClosestCollisionApproximation)
{
using obstacle_velocity_limiter::CollisionChecker;
using obstacle_velocity_limiter::distanceToClosestCollision;
using obstacle_velocity_limiter::linestring_t;
using obstacle_velocity_limiter::polygon_t;

obstacle_velocity_limiter::ProjectionParameters params;
params.distance_method = obstacle_velocity_limiter::ProjectionParameters::APPROXIMATION;
params.heading = 0.0;
linestring_t vector = {{0.0, 0.0}, {5.0, 0.0}};
polygon_t footprint;
footprint.outer() = {{0.0, 1.0}, {5.0, 1.0}, {5.0, -1.0}, {0.0, -1.0}};
boost::geometry::correct(footprint); // avoid bugs with malformed polygon
obstacle_velocity_limiter::Obstacles obstacles;

auto EPS = 1e-2;

std::optional<double> result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());
// Non Value obstacles
obstacles.points.emplace_back(-1.0, 0.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(1.0, 2.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

// inside the polygon
obstacles.points.emplace_back(4.0, 0.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_DOUBLE_EQ(*result, 4.0);

obstacles.points.emplace_back(3.0, 0.5);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 3.04, EPS);

obstacles.points.emplace_back(2.5, -0.75);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 2.61, EPS);

obstacles.points.emplace_back(2.0, -1.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 2.23, EPS);

// Change vector and footprint
vector = linestring_t{{0.0, 0.0}, {5.0, 5.0}};
params.heading = M_PI_4;
footprint.outer() = {{-1.0, 1.0}, {4.0, 6.0}, {6.0, 4.0}, {1.0, -1.0}};
boost::geometry::correct(footprint); // avoid bugs with malformed polygon
obstacles.points.clear();
obstacles.lines.clear();

// auto EPS = 1e-3;
obstacles.points.emplace_back(5.0, 1.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(4.0, 4.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 5.65, EPS);

obstacles.points.emplace_back(1.0, 2.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 2.23, EPS);

// Change vector (opposite direction)
params.heading = -3 * M_PI_4;
vector = linestring_t{{5.0, 5.0}, {0.0, 0.0}};
obstacles.points.clear();
obstacles.lines.clear();

obstacles.points.emplace_back(1.0, 1.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 5.65, EPS);

obstacles.points.emplace_back(4.0, 3.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 2.23, EPS);
}

TEST(TestCollisionDistance, distanceToClosestCollisionBicycleModel)
{
using obstacle_velocity_limiter::CollisionChecker;
using obstacle_velocity_limiter::distanceToClosestCollision;
using obstacle_velocity_limiter::linestring_t;
using obstacle_velocity_limiter::polygon_t;

obstacle_velocity_limiter::ProjectionParameters params;
params.model = obstacle_velocity_limiter::ProjectionParameters::BICYCLE;
params.heading = 0.0;
linestring_t vector = {{0.0, 0.0}, {5.0, 0.0}};
polygon_t footprint;
footprint.outer() = {{0.0, 1.0}, {5.0, 1.0}, {5.0, -1.0}, {0.0, -1.0}};
boost::geometry::correct(footprint); // avoid bugs with malformed polygon
obstacle_velocity_limiter::Obstacles obstacles;

auto EPS = 1e-2;

std::optional<double> result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(-1.0, 0.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(1.0, 2.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(4.0, 0.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(3.0, 0.5);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 3.05, EPS);

obstacles.points.emplace_back(2.5, -0.75);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 2.64, EPS);

// Change vector and footprint
vector = linestring_t{{0.0, 0.0}, {5.0, 5.0}};
params.heading = M_PI_2;
footprint.outer() = {{-1.0, 1.0}, {4.0, 6.0}, {6.0, 4.0}, {1.0, -1.0}};
boost::geometry::correct(footprint); // avoid bugs with malformed polygon
obstacles.points.clear();
obstacles.lines.clear();

result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_FALSE(result.has_value());

obstacles.points.emplace_back(4.0, 4.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 6.28, EPS);

obstacles.points.emplace_back(1.0, 2.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 2.31, EPS);

// Change vector (opposite direction)
params.heading = -M_PI;
vector = linestring_t{{5.0, 5.0}, {0.0, 0.0}};
obstacles.points.clear();
obstacles.lines.clear();

obstacles.points.emplace_back(1.0, 1.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 6.28, EPS);

obstacles.points.emplace_back(4.0, 3.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 2.76, EPS);

// change vector and footprint
params.heading = M_PI_2;
vector = linestring_t{{3.0, 3.0}, {0.0, 3.0}};
footprint.outer() = {{1.0, -1.0}, {-4.0, 6.0}, {-5.0, -4.0}, {1.0, -4.0}};
boost::geometry::correct(footprint);
obstacles.points.clear();
obstacles.lines.clear();

obstacles.points.emplace_back(-2.0, -1.0);
result =
distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 7.34, EPS);
}

TEST(TestCollisionDistance, arcDistance)
{
using obstacle_velocity_limiter::arcDistance;
Expand Down

0 comments on commit 8f07678

Please sign in to comment.