Skip to content

Commit

Permalink
test(bpp_common): add unit test for object filtering (#9408)
Browse files Browse the repository at this point in the history 
* add unit test for all function

Signed-off-by: Go Sakayori <[email protected]>

* add function to create bounding nox object

Signed-off-by: Go Sakayori <[email protected]>

---------

Signed-off-by: Go Sakayori <[email protected]>
  • Loading branch information
@go-sakayori
go-sakayori authored Nov 25, 2024
1 parent b075d50 commit 81dab81
Showing 1 changed file with 272 additions and 29 deletions.
301 changes: 272 additions & 29 deletions ...havior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,12 +14,21 @@

#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
#include "autoware/behavior_path_planner_common/utils/utils.hpp"

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>

#include <geometry_msgs/msg/detail/pose__struct.hpp>

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <lanelet2_core/Forward.h>

#include <cmath>
#include <memory>
#include <vector>

using PredictedObject = autoware_perception_msgs::msg::PredictedObject;
using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects;
Expand All @@ -32,10 +41,34 @@ using tier4_planning_msgs::msg::PathPointWithLaneId;

using autoware::test_utils::createPose;
using autoware::test_utils::generateTrajectory;
using autoware::test_utils::make_lanelet;
using autoware::universe_utils::createPoint;
using autoware::universe_utils::createVector3;

constexpr double epsilon = 1e-6;

const auto intersection_map =
autoware::test_utils::make_map_bin_msg(autoware::test_utils::get_absolute_path_to_lanelet_map(
"autoware_test_utils", "intersection/lanelet2_map.osm"));

PredictedObject create_bounding_box_object(
const geometry_msgs::msg::Pose pose,
const geometry_msgs::msg::Vector3 velocity = geometry_msgs::msg::Vector3(),
const double x_dimension = 1.0, const double y_dimension = 1.0,
const std::vector<ObjectClassification> & classification = std::vector<ObjectClassification>())
{
PredictedObject object;
object.object_id = autoware::universe_utils::generateUUID();
object.kinematics.initial_pose_with_covariance.pose = pose;
object.kinematics.initial_twist_with_covariance.twist.linear = velocity;
object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
object.shape.dimensions.x = x_dimension;
object.shape.dimensions.y = y_dimension;
object.classification = classification;

return object;
}

std::vector<PathPointWithLaneId> trajectory_to_path_with_lane_id(const Trajectory & trajectory)
{
std::vector<PathPointWithLaneId> path_with_lane_id;
Expand Down Expand Up @@ -81,8 +114,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, position_filter)
using autoware::behavior_path_planner::utils::path_safety_checker::filter::position_filter;

auto current_pos = createPoint(0.0, 0.0, 0.0);
PredictedObject object;
object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0);
PredictedObject object = create_bounding_box_object(createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0));
auto straight_trajectory = generateTrajectory<Trajectory>(20, 1.0);
double forward_distance = 20.0;
double backward_distance = 1.0;
Expand Down Expand Up @@ -129,23 +161,101 @@ TEST(BehaviorPathPlanningObjectsFiltering, is_within_circle)
EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius));
}

TEST(BehaviorPathPlanningObjectsFiltering, isCentroidWithinLanelet)
{
using autoware::behavior_path_planner::utils::path_safety_checker::isCentroidWithinLanelet;
using autoware::behavior_path_planner::utils::path_safety_checker::isCentroidWithinLanelets;

auto object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0));
auto lanelet = make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0});
double yaw_threshold = M_PI_2;

EXPECT_TRUE(isCentroidWithinLanelet(object, lanelet, yaw_threshold));

object.kinematics.initial_pose_with_covariance.pose.position.x = 8.0;
EXPECT_FALSE(isCentroidWithinLanelet(object, lanelet, yaw_threshold));

lanelet::ConstLanelets target_lanelets;
target_lanelets.push_back(lanelet);
target_lanelets.push_back(make_lanelet({5.0, 1.0}, {10.0, 1.0}, {5.0, -1.0}, {10.0, -1.0}));
EXPECT_TRUE(isCentroidWithinLanelets(object, target_lanelets));
}

TEST(BehaviorPathPlanningObjectsFiltering, isPolygonOverlapLanelet)
{
using autoware::behavior_path_planner::utils::toPolygon2d;
using autoware::behavior_path_planner::utils::path_safety_checker::isPolygonOverlapLanelet;

PredictedObject object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0));

auto lanelet = make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0});
double yaw_threshold = M_PI_2;

EXPECT_TRUE(isPolygonOverlapLanelet(object, lanelet.polygon2d().basicPolygon()));
EXPECT_TRUE(isPolygonOverlapLanelet(object, toPolygon2d(lanelet)));
EXPECT_TRUE(isPolygonOverlapLanelet(object, lanelet, yaw_threshold));

object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 10.0, 0.0, 0.0, 0.0, 0.0);
EXPECT_FALSE(isPolygonOverlapLanelet(object, lanelet.polygon2d().basicPolygon()));
EXPECT_FALSE(isPolygonOverlapLanelet(object, toPolygon2d(lanelet)));
EXPECT_FALSE(isPolygonOverlapLanelet(object, lanelet, yaw_threshold));
}

TEST(BehaviorPathPlanningObjectsFiltering, filterObjects)
{
using autoware::behavior_path_planner::utils::path_safety_checker::filterObjects;
using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
using autoware::universe_utils::createVector3;

std::shared_ptr<PredictedObjects> objects = std::make_shared<PredictedObjects>();
std::shared_ptr<autoware::route_handler::RouteHandler> route_handler =
std::make_shared<autoware::route_handler::RouteHandler>();
std::shared_ptr<ObjectsFilteringParams> params = std::make_shared<ObjectsFilteringParams>();
params->ignore_object_velocity_threshold = false;
params->object_check_forward_distance = 20.0;
params->object_check_backward_distance = 10.0;
params->object_types_to_check.check_car = true;
route_handler->setMap(intersection_map);
lanelet::ConstLanelets current_lanes;

current_lanes.push_back(route_handler->getLaneletsFromId(1000));
current_lanes.push_back(route_handler->getLaneletsFromId(1010));
auto current_pose = createPoint(360.22, 600.51, 0.0);

EXPECT_TRUE(
filterObjects(objects, route_handler, current_lanes, current_pose, params).objects.empty());

ObjectClassification classification;
classification.label = ObjectClassification::Type::CAR;
classification.probability = 1.0;

auto target_object = create_bounding_box_object(
createPose(360.22, 605.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0));
target_object.classification.push_back(classification);
auto other_object = create_bounding_box_object(
createPose(370.22, 600.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0));
other_object.classification.push_back(classification);

objects->objects.push_back(target_object);
objects->objects.push_back(other_object);

auto filtered_object = filterObjects(objects, route_handler, current_lanes, current_pose, params);
EXPECT_FALSE(filtered_object.objects.empty());
EXPECT_EQ(filtered_object.objects.front().object_id, target_object.object_id);
}

TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByVelocity)
{
using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByVelocity;

PredictedObjects objects;
PredictedObject slow_obj;
PredictedObject fast_obj;
auto slow_obj = create_bounding_box_object(
createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0));
auto fast_obj = create_bounding_box_object(
createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(10.0, 0.0, 0.0));
double vel_thr = 5.0;

slow_obj.object_id = autoware::universe_utils::generateUUID();
slow_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0;
slow_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0;
objects.objects.push_back(slow_obj);

fast_obj.object_id = autoware::universe_utils::generateUUID();
fast_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 10.0;
fast_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0;
objects.objects.push_back(fast_obj);

auto filtered_obj = filterObjectsByVelocity(objects, vel_thr, false);
Expand Down Expand Up @@ -176,17 +286,12 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition)
double search_radius = 10.0;

PredictedObjects objects;
PredictedObject far_obj;
PredictedObject near_obj;
auto far_obj = create_bounding_box_object(createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0));
auto near_obj = create_bounding_box_object(createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0));

near_obj.object_id = autoware::universe_utils::generateUUID();
near_obj.kinematics.initial_pose_with_covariance.pose = createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0);
objects.objects.push_back(near_obj);
auto target_uuid = near_obj.object_id;

far_obj.object_id = autoware::universe_utils::generateUUID();
far_obj.kinematics.initial_pose_with_covariance.pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0);
objects.objects.push_back(far_obj);
auto target_uuid = near_obj.object_id;

filterObjectsByPosition(objects, straight_path, current_pos, forward_distance, backward_distance);

Expand All @@ -202,6 +307,108 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition)
EXPECT_EQ(objects.objects.front().object_id, target_uuid);
}

TEST(BehaviorPathPlanningObjectsFiltering, separateObjectsByLanelets)
{
using autoware::behavior_path_planner::utils::path_safety_checker::isPolygonOverlapLanelet;
using autoware::behavior_path_planner::utils::path_safety_checker::
separateObjectIndicesByLanelets;
using autoware::behavior_path_planner::utils::path_safety_checker::separateObjectsByLanelets;

double yaw_threshold = M_PI_2;

auto target_object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0));
auto other_object = create_bounding_box_object(createPose(-1.5, 0.0, 0.0, 0.0, 0.0, 0.0));

PredictedObjects objects;
objects.objects.push_back(target_object);
objects.objects.push_back(other_object);

lanelet::ConstLanelets target_lanelets;
{
auto object_indices = separateObjectIndicesByLanelets(
objects, target_lanelets,
[](const auto & obj, const auto & lane, const auto & yaw_threshold) {
return isPolygonOverlapLanelet(obj, lane, yaw_threshold);
},
yaw_threshold);
EXPECT_TRUE(object_indices.first.empty());
EXPECT_TRUE(object_indices.second.empty());
}
{
target_lanelets.push_back(make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0}));
target_lanelets.push_back(make_lanelet({5.0, 1.0}, {10.0, 1.0}, {5.0, -1.0}, {10.0, -1.0}));
auto object_indices = separateObjectIndicesByLanelets(
objects, target_lanelets,
[](const auto & obj, const auto & lane, const auto & yaw_threshold) {
return isPolygonOverlapLanelet(obj, lane, yaw_threshold);
},
yaw_threshold);
EXPECT_FALSE(object_indices.first.empty());
EXPECT_FALSE(object_indices.second.empty());
EXPECT_EQ(object_indices.first.front(), 0);
EXPECT_EQ(object_indices.second.front(), 1);

auto filtered_object = separateObjectsByLanelets(
objects, target_lanelets,
[](const auto & obj, const auto & lane, const auto & yaw_threshold) {
return isPolygonOverlapLanelet(obj, lane, yaw_threshold);
},
yaw_threshold);
EXPECT_FALSE(filtered_object.first.objects.empty());
EXPECT_FALSE(filtered_object.second.objects.empty());
EXPECT_EQ(filtered_object.first.objects.front().object_id, target_object.object_id);
EXPECT_EQ(filtered_object.second.objects.front().object_id, other_object.object_id);
}
}

TEST(BehaviorPathPlanningObjectsFiltering, getPredictedPathFromObj)
{
using autoware::behavior_path_planner::utils::path_safety_checker::getPredictedPathFromObj;
using autoware::behavior_path_planner::utils::path_safety_checker::
PoseWithVelocityAndPolygonStamped;
using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;

autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject object;
std::vector<PredictedPathWithPolygon> predicted_paths;
PredictedPathWithPolygon predicted_path;

autoware_perception_msgs::msg::Shape shape;
shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
shape.dimensions.x = 1.0;
shape.dimensions.y = 1.0;

double velocity = 1.0;

const auto path = [&](geometry_msgs::msg::Pose initial_pose) {
std::vector<PoseWithVelocityAndPolygonStamped> path;
geometry_msgs::msg::Pose pose;
for (size_t i = 0; i < 10; i++) {
auto time = static_cast<double>(i);
pose.position.x = initial_pose.position.x + time * velocity;
pose.position.y = initial_pose.position.y;
PoseWithVelocityAndPolygonStamped obj_pose_with_poly(
time, pose, velocity, autoware::universe_utils::toPolygon2d(pose, shape));
path.push_back(obj_pose_with_poly);
}
return path;
};

for (size_t i = 0; i < 2; i++) {
predicted_path.path = path(createPose(0.0, static_cast<double>(i), 0.0, 0.0, 0.0, 0.0));
predicted_path.confidence = 0.1f * (static_cast<float>(i) + 1.0f);
predicted_paths.push_back(predicted_path);
}
object.predicted_paths = predicted_paths;

bool use_all_predicted_path = true;
EXPECT_EQ(getPredictedPathFromObj(object, use_all_predicted_path).size(), 2);

use_all_predicted_path = false;
auto extracted_path = getPredictedPathFromObj(object, use_all_predicted_path);
EXPECT_EQ(extracted_path.size(), 1);
EXPECT_DOUBLE_EQ(extracted_path.front().path.front().pose.position.y, 1.0);
}

TEST(BehaviorPathPlanningObjectsFiltering, createPredictedPath)
{
using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath;
Expand Down Expand Up @@ -257,22 +464,14 @@ TEST(BehaviorPathPlanningObjectsFiltering, transform)
{
using autoware::behavior_path_planner::utils::path_safety_checker::transform;
auto velocity = autoware::universe_utils::createVector3(2.0, 0.0, 0.0);
auto angular = autoware::universe_utils::createVector3(0.0, 0.0, 0.0);

PredictedObject obj;
obj.object_id = autoware::universe_utils::generateUUID();
obj.kinematics.initial_pose_with_covariance.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0);
obj.kinematics.initial_twist_with_covariance.twist =
autoware::universe_utils::createTwist(velocity, angular);
auto obj = create_bounding_box_object(
createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0), 2.0, 1.0);
autoware_perception_msgs::msg::PredictedPath predicted_path;
auto straight_path = trajectory_to_predicted_path(generateTrajectory<Trajectory>(5, 1.0));
straight_path.confidence = 0.6;
straight_path.time_step.sec = 1.0;
obj.kinematics.predicted_paths.push_back(straight_path);
obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
obj.shape.dimensions.x = 2.0;
obj.shape.dimensions.y = 1.0;
obj.shape.dimensions.z = 1.0;

double safety_check_time_horizon = 2.0;
double safety_check_time_resolution = 0.5;
Expand Down Expand Up @@ -356,6 +555,50 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByClass)
EXPECT_TRUE(objects.objects.empty());
}

TEST(BehaviorPathPlanningObjectsFiltering, createTargetObjectsOnLane)
{
using autoware::behavior_path_planner::utils::path_safety_checker::createTargetObjectsOnLane;
using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
using autoware::universe_utils::createVector3;

PredictedObjects objects;
std::shared_ptr<autoware::route_handler::RouteHandler> route_handler =
std::make_shared<autoware::route_handler::RouteHandler>();
std::shared_ptr<ObjectsFilteringParams> params = std::make_shared<ObjectsFilteringParams>();
params->object_lane_configuration = {true, true, true, true, true};
params->include_opposite_lane = true;
params->invert_opposite_lane = false;
params->safety_check_time_horizon = 10.0;
params->safety_check_time_resolution = 1.0;
route_handler->setMap(intersection_map);
lanelet::ConstLanelets current_lanes;

current_lanes.push_back(route_handler->getLaneletsFromId(1001));
current_lanes.push_back(route_handler->getLaneletsFromId(1011));

ObjectClassification classification;
classification.label = ObjectClassification::Type::CAR;
classification.probability = 1.0;

PredictedObject current_lane_object =
create_bounding_box_object(createPose(363.64, 565.03, 0.0, 0.0, 0.0, 0.0));
PredictedObject right_lane_object =
create_bounding_box_object(createPose(366.91, 523.47, 0.0, 0.0, 0.0, 0.0));

objects.objects.push_back(current_lane_object);
objects.objects.push_back(right_lane_object);

auto target_objects_on_lane =
createTargetObjectsOnLane(current_lanes, route_handler, objects, params);
EXPECT_FALSE(target_objects_on_lane.on_current_lane.empty());
EXPECT_FALSE(target_objects_on_lane.on_right_lane.empty());
EXPECT_TRUE(target_objects_on_lane.on_left_lane.empty());
EXPECT_TRUE(target_objects_on_lane.on_other_lane.empty());

EXPECT_EQ(target_objects_on_lane.on_current_lane.front().uuid, current_lane_object.object_id);
EXPECT_EQ(target_objects_on_lane.on_right_lane.front().uuid, right_lane_object.object_id);
}

TEST(BehaviorPathPlanningObjectsFiltering, isTargetObjectType)
{
using autoware::behavior_path_planner::utils::path_safety_checker::isTargetObjectType;
Expand Down

0 comments on commit 81dab81

Please sign in to comment.