Skip to content

Commit 4e9755b

Browse files
takayuki5168takayuki5168
authored
feat(start_planner): add time_keeper (autowarefoundation#8254)
* feat(start_planner): add time_keeper Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix shadow variables Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent da61d22 commit 4e9755b

File tree

10 files changed

+122
-38
lines changed

10 files changed

+122
-38
lines changed

‎control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp

+13-4
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include <autoware/universe_utils/geometry/boost_geometry.hpp>
1919
#include <autoware/universe_utils/geometry/pose_deviation.hpp>
20+
#include <autoware/universe_utils/system/time_keeper.hpp>
2021
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
2122
#include <rosidl_runtime_cpp/message_initialization.hpp>
2223

@@ -101,6 +102,12 @@ struct Output
101102
class LaneDepartureChecker
102103
{
103104
public:
105+
LaneDepartureChecker(
106+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
107+
std::make_shared<universe_utils::TimeKeeper>())
108+
: time_keeper_(time_keeper)
109+
{
110+
}
104111
Output update(const Input & input);
105112

106113
void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info)
@@ -156,19 +163,21 @@ class LaneDepartureChecker
156163
static std::vector<LinearRing2d> createVehiclePassingAreas(
157164
const std::vector<LinearRing2d> & vehicle_footprints);
158165

159-
static bool willLeaveLane(
166+
bool willLeaveLane(
160167
const lanelet::ConstLanelets & candidate_lanelets,
161-
const std::vector<LinearRing2d> & vehicle_footprints);
168+
const std::vector<LinearRing2d> & vehicle_footprints) const;
162169

163170
double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const;
164171

165172
static SegmentRtree extractUncrossableBoundaries(
166173
const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point,
167174
const double max_search_length, const std::vector<std::string> & boundary_types_to_detect);
168175

169-
static bool willCrossBoundary(
176+
bool willCrossBoundary(
170177
const std::vector<LinearRing2d> & vehicle_footprints,
171-
const SegmentRtree & uncrossable_segments);
178+
const SegmentRtree & uncrossable_segments) const;
179+
180+
mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
172181
};
173182
} // namespace autoware::lane_departure_checker
174183

‎control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

+31-9
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,8 @@ Output LaneDepartureChecker::update(const Input & input)
160160
bool LaneDepartureChecker::checkPathWillLeaveLane(
161161
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const
162162
{
163+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
164+
163165
std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
164166
lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
165167
return willLeaveLane(candidate_lanelets, vehicle_footprints);
@@ -290,8 +292,10 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
290292

291293
bool LaneDepartureChecker::willLeaveLane(
292294
const lanelet::ConstLanelets & candidate_lanelets,
293-
const std::vector<LinearRing2d> & vehicle_footprints)
295+
const std::vector<LinearRing2d> & vehicle_footprints) const
294296
{
297+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
298+
295299
for (const auto & vehicle_footprint : vehicle_footprints) {
296300
if (isOutOfLane(candidate_lanelets, vehicle_footprint)) {
297301
return true;
@@ -304,6 +308,8 @@ bool LaneDepartureChecker::willLeaveLane(
304308
std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
305309
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
306310
{
311+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
312+
307313
// Get Footprint Hull basic polygon
308314
std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
309315
LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
@@ -326,6 +332,8 @@ std::optional<autoware::universe_utils::Polygon2d>
326332
LaneDepartureChecker::getFusedLaneletPolygonForPath(
327333
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
328334
{
335+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
336+
329337
const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
330338
auto to_polygon2d =
331339
[](const lanelet::BasicPolygon2d & poly) -> autoware::universe_utils::Polygon2d {
@@ -365,6 +373,8 @@ LaneDepartureChecker::getFusedLaneletPolygonForPath(
365373
bool LaneDepartureChecker::checkPathWillLeaveLane(
366374
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
367375
{
376+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
377+
368378
// check if the footprint is not fully contained within the fused lanelets polygon
369379
const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
370380
const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
@@ -379,17 +389,26 @@ bool LaneDepartureChecker::checkPathWillLeaveLane(
379389
PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
380390
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
381391
{
392+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
393+
382394
PathWithLaneId temp_path;
383395
const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
384396
if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
385397
const auto vehicle_footprints = createVehicleFootprints(path);
386-
size_t idx = 0;
387-
std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
388-
if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
389-
temp_path.points.push_back(path.points.at(idx));
390-
}
391-
++idx;
392-
});
398+
399+
{
400+
universe_utils::ScopedTimeTrack st2(
401+
"check if footprint is within fused_lanlets_polygon", *time_keeper_);
402+
403+
size_t idx = 0;
404+
std::for_each(
405+
vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
406+
if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
407+
temp_path.points.push_back(path.points.at(idx));
408+
}
409+
++idx;
410+
});
411+
}
393412
PathWithLaneId cropped_path = path;
394413
cropped_path.points = temp_path.points;
395414
return cropped_path;
@@ -449,8 +468,11 @@ SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries(
449468
}
450469

451470
bool LaneDepartureChecker::willCrossBoundary(
452-
const std::vector<LinearRing2d> & vehicle_footprints, const SegmentRtree & uncrossable_segments)
471+
const std::vector<LinearRing2d> & vehicle_footprints,
472+
const SegmentRtree & uncrossable_segments) const
453473
{
474+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
475+
454476
for (const auto & footprint : vehicle_footprints) {
455477
std::vector<Segment2d> intersection_result;
456478
uncrossable_segments.query(

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

+3-1
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_
1717

1818
#include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp"
19+
#include "autoware/universe_utils/system/time_keeper.hpp"
1920

2021
#include <autoware/freespace_planning_algorithms/abstract_algorithm.hpp>
2122
#include <autoware/freespace_planning_algorithms/astar_search.hpp>
@@ -36,7 +37,8 @@ class FreespacePullOut : public PullOutPlannerBase
3637
public:
3738
FreespacePullOut(
3839
rclcpp::Node & node, const StartPlannerParameters & parameters,
39-
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
40+
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
41+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
4042

4143
PlannerType getPlannerType() const override { return PlannerType::FREESPACE; }
4244

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

+3-1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp"
1919
#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
2020
#include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp"
21+
#include "autoware/universe_utils/system/time_keeper.hpp"
2122

2223
#include <autoware/lane_departure_checker/lane_departure_checker.hpp>
2324

@@ -33,7 +34,8 @@ class GeometricPullOut : public PullOutPlannerBase
3334
explicit GeometricPullOut(
3435
rclcpp::Node & node, const StartPlannerParameters & parameters,
3536
const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
36-
lane_departure_checker);
37+
lane_departure_checker,
38+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
3739

3840
PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; };
3941
std::optional<PullOutPath> plan(

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

+9-1
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include "autoware/behavior_path_start_planner_module/data_structs.hpp"
2121
#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
2222
#include "autoware/behavior_path_start_planner_module/util.hpp"
23+
#include "autoware/universe_utils/system/time_keeper.hpp"
2324

2425
#include <geometry_msgs/msg/pose_stamped.hpp>
2526
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
@@ -37,7 +38,10 @@ using tier4_planning_msgs::msg::PathWithLaneId;
3738
class PullOutPlannerBase
3839
{
3940
public:
40-
explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters)
41+
explicit PullOutPlannerBase(
42+
rclcpp::Node & node, const StartPlannerParameters & parameters,
43+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
44+
: time_keeper_(time_keeper)
4145
{
4246
vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
4347
vehicle_footprint_ = vehicle_info_.createFootprint();
@@ -63,6 +67,8 @@ class PullOutPlannerBase
6367
autoware::behavior_path_planner::PullOutPath & pull_out_path,
6468
double collision_check_distance_from_end) const
6569
{
70+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
71+
6672
// check for collisions
6773
const auto & dynamic_objects = planner_data_->dynamic_object;
6874
const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
@@ -95,6 +101,8 @@ class PullOutPlannerBase
95101
LinearRing2d vehicle_footprint_;
96102
StartPlannerParameters parameters_;
97103
double collision_check_margin_;
104+
105+
mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
98106
};
99107
} // namespace autoware::behavior_path_planner
100108

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

+3-1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
1919
#include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp"
20+
#include "autoware/universe_utils/system/time_keeper.hpp"
2021

2122
#include <autoware/lane_departure_checker/lane_departure_checker.hpp>
2223

@@ -34,7 +35,8 @@ class ShiftPullOut : public PullOutPlannerBase
3435
public:
3536
explicit ShiftPullOut(
3637
rclcpp::Node & node, const StartPlannerParameters & parameters,
37-
std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
38+
std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
39+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
3840

3941
PlannerType getPlannerType() const override { return PlannerType::SHIFT; };
4042
std::optional<PullOutPath> plan(

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

+4-2
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,10 @@ namespace autoware::behavior_path_planner
2828
{
2929
FreespacePullOut::FreespacePullOut(
3030
rclcpp::Node & node, const StartPlannerParameters & parameters,
31-
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
32-
: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity}
31+
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
32+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
33+
: PullOutPlannerBase{node, parameters, time_keeper},
34+
velocity_{parameters.freespace_planner_velocity}
3335
{
3436
autoware::freespace_planning_algorithms::VehicleShape vehicle_shape(
3537
vehicle_info, parameters.vehicle_shape_margin);

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

+3-2
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,9 @@ using start_planner_utils::getPullOutLanes;
3333
GeometricPullOut::GeometricPullOut(
3434
rclcpp::Node & node, const StartPlannerParameters & parameters,
3535
const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
36-
lane_departure_checker)
37-
: PullOutPlannerBase{node, parameters},
36+
lane_departure_checker,
37+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
38+
: PullOutPlannerBase{node, parameters, time_keeper},
3839
parallel_parking_parameters_{parameters.parallel_parking_parameters},
3940
lane_departure_checker_(lane_departure_checker)
4041
{

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

+7-2
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,9 @@ using start_planner_utils::getPullOutLanes;
3737

3838
ShiftPullOut::ShiftPullOut(
3939
rclcpp::Node & node, const StartPlannerParameters & parameters,
40-
std::shared_ptr<LaneDepartureChecker> & lane_departure_checker)
41-
: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}
40+
std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
41+
std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
42+
: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker}
4243
{
4344
}
4445

@@ -62,6 +63,8 @@ std::optional<PullOutPath> ShiftPullOut::plan(
6263

6364
// get safe path
6465
for (auto & pull_out_path : pull_out_paths) {
66+
universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_);
67+
6568
// shift path is not separate but only one.
6669
auto & shift_path = pull_out_path.partial_paths.front();
6770
// check lane_departure with path between pull_out_start to pull_out_end
@@ -215,6 +218,8 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
215218
const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
216219
const Pose & start_pose, const Pose & goal_pose)
217220
{
221+
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
222+
218223
std::vector<PullOutPath> candidate_paths{};
219224

220225
if (road_lanes.empty()) {

0 commit comments

Comments
 (0)