Skip to content

Commit 9546c0e

Browse files
kyoichi-sugaharakyoichi-sugahara
authored
test(autoware_behavior_path_start_planner_module): add test helper and implement unit tests for FreespacePullOut (#9832)
* refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners Signed-off-by: Kyoichi Sugahara <[email protected]> * refactor(autoware_behavior_path_start_planner_module): remove TimeKeeper parameter from pull-out planners Signed-off-by: Kyoichi Sugahara <[email protected]> * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling Signed-off-by: kyoichi-sugahara <[email protected]> * refactor(planner): add planner_data parameter to plan methods in pull out planners Signed-off-by: kyoichi-sugahara <[email protected]> * refactor(autoware_behavior_path_start_planner_module): remove LaneDepartureChecker dependency from pull-out planners Signed-off-by: Kyoichi Sugahara <[email protected]> --------- Signed-off-by: Kyoichi Sugahara <[email protected]>
1 parent 7d63242 commit 9546c0e

26 files changed

+385
-277
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,6 @@
2323
#include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
2424
#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
2525

26-
#include <autoware/lane_departure_checker/lane_departure_checker.hpp>
27-
2826
#include <autoware_vehicle_msgs/msg/hazard_lights_command.hpp>
2927
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
3028

@@ -40,7 +38,6 @@
4038

4139
namespace autoware::behavior_path_planner
4240
{
43-
using autoware::lane_departure_checker::LaneDepartureChecker;
4441
using autoware_vehicle_msgs::msg::HazardLightsCommand;
4542
using geometry_msgs::msg::PoseArray;
4643
using nav_msgs::msg::OccupancyGrid;

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp

+1-3
Original file line numberDiff line numberDiff line change
@@ -29,9 +29,7 @@ using autoware::lane_departure_checker::LaneDepartureChecker;
2929
class BezierPullOver : public PullOverPlannerBase
3030
{
3131
public:
32-
BezierPullOver(
33-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
34-
const LaneDepartureChecker & lane_departure_checker);
32+
BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters);
3533
PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::BEZIER; }
3634
std::optional<PullOverPath> plan(
3735
const GoalCandidate & modified_goal_pose, const size_t id,

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp

+1-3
Original file line numberDiff line numberDiff line change
@@ -30,9 +30,7 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm;
3030
class FreespacePullOver : public PullOverPlannerBase
3131
{
3232
public:
33-
FreespacePullOver(
34-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
35-
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
33+
FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters);
3634

3735
PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; }
3836

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,7 @@ class GeometricPullOver : public PullOverPlannerBase
3232
{
3333
public:
3434
GeometricPullOver(
35-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
36-
const LaneDepartureChecker & lane_departure_checker, const bool is_forward);
35+
rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward);
3736

3837
PullOverPlannerType getPlannerType() const override
3938
{

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp

+1-3
Original file line numberDiff line numberDiff line change
@@ -31,9 +31,7 @@ using autoware::lane_departure_checker::LaneDepartureChecker;
3131
class ShiftPullOver : public PullOverPlannerBase
3232
{
3333
public:
34-
ShiftPullOver(
35-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
36-
const LaneDepartureChecker & lane_departure_checker);
34+
ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters);
3735
PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; };
3836
std::optional<PullOverPath> plan(
3937
const GoalCandidate & modified_goal_pose, const size_t id,

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

+6-13
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ GoalPlannerModule::GoalPlannerModule(
101101

102102
// freespace parking
103103
if (parameters_->enable_freespace_parking) {
104-
auto freespace_planner = std::make_shared<FreespacePullOver>(node, *parameters, vehicle_info);
104+
auto freespace_planner = std::make_shared<FreespacePullOver>(node, *parameters);
105105
const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period();
106106
freespace_parking_timer_cb_group_ =
107107
node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -280,22 +280,15 @@ LaneParkingPlanner::LaneParkingPlanner(
280280
is_lane_parking_cb_running_(is_lane_parking_cb_running),
281281
logger_(logger)
282282
{
283-
const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
284-
lane_departure_checker::Param lane_departure_checker_params;
285-
lane_departure_checker_params.footprint_extra_margin =
286-
parameters.lane_departure_check_expansion_margin;
287-
LaneDepartureChecker lane_departure_checker(lane_departure_checker_params, vehicle_info);
288-
289283
for (const std::string & planner_type : parameters.efficient_path_order) {
290284
if (planner_type == "SHIFT" && parameters.enable_shift_parking) {
291-
pull_over_planners_.push_back(
292-
std::make_shared<ShiftPullOver>(node, parameters, lane_departure_checker));
285+
pull_over_planners_.push_back(std::make_shared<ShiftPullOver>(node, parameters));
293286
} else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) {
294-
pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
295-
node, parameters, lane_departure_checker, /*is_forward*/ true));
287+
pull_over_planners_.push_back(
288+
std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ true));
296289
} else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) {
297-
pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
298-
node, parameters, lane_departure_checker, /*is_forward*/ false));
290+
pull_over_planners_.push_back(
291+
std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ false));
299292
}
300293
}
301294

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp

+5-7
Original file line numberDiff line numberDiff line change
@@ -31,15 +31,13 @@
3131
#include <vector>
3232
namespace autoware::behavior_path_planner
3333
{
34-
BezierPullOver::BezierPullOver(
35-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
36-
const LaneDepartureChecker & lane_departure_checker)
37-
: PullOverPlannerBase{node, parameters},
38-
lane_departure_checker_{lane_departure_checker},
39-
left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}
34+
BezierPullOver::BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters)
35+
: PullOverPlannerBase(node, parameters),
36+
lane_departure_checker_(
37+
lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_),
38+
left_side_parking_(parameters.parking_policy == ParkingPolicy::LEFT_SIDE)
4039
{
4140
}
42-
4341
std::optional<PullOverPath> BezierPullOver::plan(
4442
[[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id,
4543
[[maybe_unused]] const std::shared_ptr<const PlannerData> planner_data,

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -32,9 +32,7 @@ namespace autoware::behavior_path_planner
3232
using autoware::freespace_planning_algorithms::AstarSearch;
3333
using autoware::freespace_planning_algorithms::RRTStar;
3434

35-
FreespacePullOver::FreespacePullOver(
36-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
37-
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
35+
FreespacePullOver::FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters)
3836
: PullOverPlannerBase{node, parameters},
3937
velocity_{parameters.freespace_parking_velocity},
4038
left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE},
@@ -45,7 +43,7 @@ FreespacePullOver::FreespacePullOver(
4543
}
4644
{
4745
autoware::freespace_planning_algorithms::VehicleShape vehicle_shape(
48-
vehicle_info, parameters.vehicle_shape_margin);
46+
vehicle_info_, parameters.vehicle_shape_margin);
4947
if (parameters.freespace_parking_algorithm == "astar") {
5048
planner_ = std::make_unique<AstarSearch>(
5149
parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters,

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

+3-4
Original file line numberDiff line numberDiff line change
@@ -26,11 +26,10 @@
2626
namespace autoware::behavior_path_planner
2727
{
2828
GeometricPullOver::GeometricPullOver(
29-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
30-
const LaneDepartureChecker & lane_departure_checker, const bool is_forward)
29+
rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward)
3130
: PullOverPlannerBase{node, parameters},
32-
parallel_parking_parameters_{parameters.parallel_parking_parameters},
33-
lane_departure_checker_{lane_departure_checker},
31+
lane_departure_checker_{
32+
lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_},
3433
is_forward_{is_forward},
3534
left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}
3635
{

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

+3-4
Original file line numberDiff line numberDiff line change
@@ -30,11 +30,10 @@
3030

3131
namespace autoware::behavior_path_planner
3232
{
33-
ShiftPullOver::ShiftPullOver(
34-
rclcpp::Node & node, const GoalPlannerParameters & parameters,
35-
const LaneDepartureChecker & lane_departure_checker)
33+
ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters)
3634
: PullOverPlannerBase{node, parameters},
37-
lane_departure_checker_{lane_departure_checker},
35+
lane_departure_checker_{
36+
lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_},
3837
left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}
3938
{
4039
}

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt

+12-2
Original file line numberDiff line numberDiff line change
@@ -19,12 +19,22 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
1919
if(BUILD_TESTING)
2020
find_package(ament_lint_auto REQUIRED)
2121
ament_lint_auto_find_test_dependencies()
22+
23+
# Add test helper library
24+
ament_auto_add_library(start_planner_test_helper SHARED
25+
test/src/start_planner_test_helper.cpp
26+
)
27+
target_include_directories(start_planner_test_helper PUBLIC
28+
test/include
29+
)
30+
2231
file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
2332
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
2433
${TEST_SOURCES}
2534
)
26-
target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
35+
target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}
36+
start_planner_test_helper
37+
)
2738
endif()
2839

29-
3040
ament_auto_package(INSTALL_TO_SHARE config)

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,11 @@ class FreespacePullOut : public PullOutPlannerBase
4040
PlannerType getPlannerType() const override { return PlannerType::FREESPACE; }
4141

4242
std::optional<PullOutPath> plan(
43-
const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) override;
43+
const Pose & start_pose, const Pose & end_pose,
44+
const std::shared_ptr<const PlannerData> & planner_data,
45+
PlannerDebugData & planner_debug_data) override;
46+
47+
friend class TestFreespacePullOut;
4448

4549
protected:
4650
std::unique_ptr<AbstractPlanningAlgorithm> planner_;

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -33,14 +33,13 @@ class GeometricPullOut : public PullOutPlannerBase
3333
public:
3434
explicit GeometricPullOut(
3535
rclcpp::Node & node, const StartPlannerParameters & parameters,
36-
const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
37-
lane_departure_checker,
3836
std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
3937
std::make_shared<universe_utils::TimeKeeper>());
4038

4139
PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; };
4240
std::optional<PullOutPath> plan(
4341
const Pose & start_pose, const Pose & goal_pose,
42+
const std::shared_ptr<const PlannerData> & planner_data,
4443
PlannerDebugData & planner_debug_data) override;
4544

4645
GeometricParallelParking planner_;

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp

+7-10
Original file line numberDiff line numberDiff line change
@@ -42,33 +42,30 @@ class PullOutPlannerBase
4242
rclcpp::Node & node, const StartPlannerParameters & parameters,
4343
std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
4444
std::make_shared<universe_utils::TimeKeeper>())
45-
: parameters_(parameters),
46-
vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()),
47-
vehicle_footprint_(vehicle_info_.createFootprint()),
45+
: parameters_{parameters},
46+
vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()},
47+
vehicle_footprint_{vehicle_info_.createFootprint()},
4848
time_keeper_(time_keeper)
4949
{
5050
}
5151
virtual ~PullOutPlannerBase() = default;
5252

53-
void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
54-
{
55-
planner_data_ = planner_data;
56-
}
57-
5853
void setCollisionCheckMargin(const double collision_check_margin)
5954
{
6055
collision_check_margin_ = collision_check_margin;
6156
};
6257
virtual PlannerType getPlannerType() const = 0;
6358
virtual std::optional<PullOutPath> plan(
64-
const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) = 0;
59+
const Pose & start_pose, const Pose & goal_pose,
60+
const std::shared_ptr<const PlannerData> & planner_data,
61+
PlannerDebugData & planner_debug_data) = 0;
6562

6663
protected:
6764
bool isPullOutPathCollided(
6865
autoware::behavior_path_planner::PullOutPath & pull_out_path,
66+
const std::shared_ptr<const PlannerData> & planner_data,
6967
double collision_check_distance_from_end) const;
7068

71-
std::shared_ptr<const PlannerData> planner_data_;
7269
StartPlannerParameters parameters_;
7370
autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
7471
LinearRing2d vehicle_footprint_;

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -35,18 +35,19 @@ class ShiftPullOut : public PullOutPlannerBase
3535
public:
3636
explicit ShiftPullOut(
3737
rclcpp::Node & node, const StartPlannerParameters & parameters,
38-
std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
3938
std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
4039
std::make_shared<universe_utils::TimeKeeper>());
4140

4241
PlannerType getPlannerType() const override { return PlannerType::SHIFT; };
4342
std::optional<PullOutPath> plan(
4443
const Pose & start_pose, const Pose & goal_pose,
44+
const std::shared_ptr<const PlannerData> & planner_data,
4545
PlannerDebugData & planner_debug_data) override;
4646

4747
std::vector<PullOutPath> calcPullOutPaths(
4848
const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
49-
const Pose & start_pose, const Pose & goal_pose);
49+
const Pose & start_pose, const Pose & goal_pose,
50+
const BehaviorPathPlannerParameters & behavior_path_parameters);
5051

5152
double calcBeforeShiftedArcLength(
5253
const PathWithLaneId & path, const double target_after_arc_length, const double dr);

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp

-4
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,6 @@
2727
#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
2828
#include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp"
2929

30-
#include <autoware/lane_departure_checker/lane_departure_checker.hpp>
3130
#include <autoware_vehicle_info_utils/vehicle_info.hpp>
3231
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
3332

@@ -51,7 +50,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter
5150
using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
5251
using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams;
5352
using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
54-
using autoware::lane_departure_checker::LaneDepartureChecker;
5553
using geometry_msgs::msg::PoseArray;
5654
using PriorityOrder = std::vector<std::pair<size_t, std::shared_ptr<PullOutPlannerBase>>>;
5755

@@ -310,8 +308,6 @@ ego pose.
310308
std::vector<Pose> searchPullOutStartPoseCandidates(
311309
const PathWithLaneId & back_path_from_start_pose) const;
312310

313-
std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
314-
315311
// turn signal
316312
TurnSignalInfo calcTurnSignalInfo();
317313

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp

+8-7
Original file line numberDiff line numberDiff line change
@@ -47,13 +47,14 @@ FreespacePullOut::FreespacePullOut(rclcpp::Node & node, const StartPlannerParame
4747
}
4848

4949
std::optional<PullOutPath> FreespacePullOut::plan(
50-
const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data)
50+
const Pose & start_pose, const Pose & end_pose,
51+
const std::shared_ptr<const PlannerData> & planner_data, PlannerDebugData & planner_debug_data)
5152
{
52-
const auto & route_handler = planner_data_->route_handler;
53-
const double backward_path_length = planner_data_->parameters.backward_path_length;
54-
const double forward_path_length = planner_data_->parameters.forward_path_length;
53+
const auto & route_handler = planner_data->route_handler;
54+
const double backward_path_length = planner_data->parameters.backward_path_length;
55+
const double forward_path_length = planner_data->parameters.forward_path_length;
5556

56-
planner_->setMap(*planner_data_->costmap);
57+
planner_->setMap(*planner_data->costmap);
5758

5859
try {
5960
if (!planner_->makePlan(start_pose, end_pose)) {
@@ -65,11 +66,11 @@ std::optional<PullOutPath> FreespacePullOut::plan(
6566
}
6667

6768
const auto road_lanes = utils::getExtendedCurrentLanes(
68-
planner_data_, backward_path_length, std::numeric_limits<double>::max(),
69+
planner_data, backward_path_length, std::numeric_limits<double>::max(),
6970
/*forward_only_in_route*/ true);
7071
// find candidate paths
7172
const auto pull_out_lanes =
72-
start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
73+
start_planner_utils::getPullOutLanes(planner_data, backward_path_length);
7374
const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes);
7475

7576
const PathWithLaneId path =

0 commit comments

Comments
 (0)