diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index b43a7d0d0369a..7e39cb6cba8c4 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -101,6 +102,12 @@ struct Output class LaneDepartureChecker { public: + LaneDepartureChecker( + std::shared_ptr time_keeper = + std::make_shared()) + : time_keeper_(time_keeper) + { + } Output update(const Input & input); void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) @@ -156,9 +163,9 @@ class LaneDepartureChecker static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); - static bool willLeaveLane( + bool willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints); + const std::vector & vehicle_footprints) const; double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const; @@ -166,9 +173,11 @@ class LaneDepartureChecker const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, const double max_search_length, const std::vector & boundary_types_to_detect); - static bool willCrossBoundary( + bool willCrossBoundary( const std::vector & vehicle_footprints, - const SegmentRtree & uncrossable_segments); + const SegmentRtree & uncrossable_segments) const; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::lane_departure_checker diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 7104d20f122fb..fcb64607f6d46 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -160,6 +160,8 @@ Output LaneDepartureChecker::update(const Input & input) bool LaneDepartureChecker::checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::vector vehicle_footprints = createVehicleFootprints(path); lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); return willLeaveLane(candidate_lanelets, vehicle_footprints); @@ -290,8 +292,10 @@ std::vector LaneDepartureChecker::createVehiclePassingAreas( bool LaneDepartureChecker::willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints) + const std::vector & vehicle_footprints) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + for (const auto & vehicle_footprint : vehicle_footprints) { if (isOutOfLane(candidate_lanelets, vehicle_footprint)) { return true; @@ -304,6 +308,8 @@ bool LaneDepartureChecker::willLeaveLane( std::vector> LaneDepartureChecker::getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // Get Footprint Hull basic polygon std::vector vehicle_footprints = createVehicleFootprints(path); LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); @@ -326,6 +332,8 @@ std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> autoware::universe_utils::Polygon2d { @@ -365,6 +373,8 @@ LaneDepartureChecker::getFusedLaneletPolygonForPath( bool LaneDepartureChecker::checkPathWillLeaveLane( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // check if the footprint is not fully contained within the fused lanelets polygon const std::vector vehicle_footprints = createVehicleFootprints(path); const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); @@ -379,17 +389,26 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + PathWithLaneId temp_path; const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); if (path.points.empty() || !fused_lanelets_polygon) return temp_path; const auto vehicle_footprints = createVehicleFootprints(path); - size_t idx = 0; - std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { - if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { - temp_path.points.push_back(path.points.at(idx)); - } - ++idx; - }); + + { + universe_utils::ScopedTimeTrack st2( + "check if footprint is within fused_lanlets_polygon", *time_keeper_); + + size_t idx = 0; + std::for_each( + vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { + if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { + temp_path.points.push_back(path.points.at(idx)); + } + ++idx; + }); + } PathWithLaneId cropped_path = path; cropped_path.points = temp_path.points; return cropped_path; @@ -449,8 +468,11 @@ SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries( } bool LaneDepartureChecker::willCrossBoundary( - const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments) + const std::vector & vehicle_footprints, + const SegmentRtree & uncrossable_segments) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + for (const auto & footprint : vehicle_footprints) { std::vector intersection_result; uncrossable_segments.query( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index 145ca5175762e..e358e35ed7794 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -36,7 +37,8 @@ class FreespacePullOut : public PullOutPlannerBase public: FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + std::shared_ptr time_keeper); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index c63dd392a5463..70ca2bdb10d37 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include @@ -33,7 +34,8 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, const std::shared_ptr - lane_departure_checker); + lane_departure_checker, + std::shared_ptr time_keeper); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 03b6e13cb2d3e..de2e57ff23576 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -20,6 +20,7 @@ #include "autoware/behavior_path_start_planner_module/data_structs.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -37,7 +38,10 @@ using tier4_planning_msgs::msg::PathWithLaneId; class PullOutPlannerBase { public: - explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) + explicit PullOutPlannerBase( + rclcpp::Node & node, const StartPlannerParameters & parameters, + std::shared_ptr time_keeper) + : time_keeper_(time_keeper) { vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); @@ -63,6 +67,8 @@ class PullOutPlannerBase autoware::behavior_path_planner::PullOutPath & pull_out_path, double collision_check_distance_from_end) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // check for collisions const auto & dynamic_objects = planner_data_->dynamic_object; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -95,6 +101,8 @@ class PullOutPlannerBase LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; double collision_check_margin_; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 92cad1c2eb8b8..d1b4c9dabdd7f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -17,6 +17,7 @@ #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include @@ -34,7 +35,8 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker); + std::shared_ptr & lane_departure_checker, + std::shared_ptr time_keeper); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 072e49d381440..c3f5e8883c284 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -28,8 +28,10 @@ namespace autoware::behavior_path_planner { FreespacePullOut::FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) -: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + std::shared_ptr time_keeper) +: PullOutPlannerBase{node, parameters, time_keeper}, + velocity_{parameters.freespace_planner_velocity} { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 4470026bc8dda..13a9908b97daf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -33,8 +33,9 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, const std::shared_ptr - lane_departure_checker) -: PullOutPlannerBase{node, parameters}, + lane_departure_checker, + std::shared_ptr time_keeper) +: PullOutPlannerBase{node, parameters, time_keeper}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_(lane_departure_checker) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 18970ead153af..2b63ff6df0db2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -37,8 +37,9 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker) -: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker} + std::shared_ptr & lane_departure_checker, + std::shared_ptr time_keeper) +: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker} { } @@ -62,6 +63,8 @@ std::optional ShiftPullOut::plan( // get safe path for (auto & pull_out_path : pull_out_paths) { + universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_); + // shift path is not separate but only one. auto & shift_path = pull_out_path.partial_paths.front(); // check lane_departure with path between pull_out_start to pull_out_end @@ -215,6 +218,8 @@ std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, const Pose & goal_pose) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::vector candidate_paths{}; if (road_lanes.empty()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index c621b16824110..5c3c62b947af2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -65,7 +65,7 @@ StartPlannerModule::StartPlannerModule( vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { - lane_departure_checker_ = std::make_shared(); + lane_departure_checker_ = std::make_shared(time_keeper_); lane_departure_checker_->setVehicleInfo(vehicle_info_); autoware::lane_departure_checker::Param lane_departure_checker_params{}; lane_departure_checker_params.footprint_extra_margin = @@ -76,18 +76,19 @@ StartPlannerModule::StartPlannerModule( // set enabled planner if (parameters_->enable_shift_pull_out) { start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_)); + std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); } if (parameters_->enable_geometric_pull_out) { start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_)); + std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } if (parameters_->enable_freespace_planner) { - freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + freespace_planner_ = + std::make_unique(node, *parameters, vehicle_info_, time_keeper_); const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); freespace_planner_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -211,6 +212,8 @@ void StartPlannerModule::updateObjectsFilteringParams( void StartPlannerModule::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // The method PlannerManager::run() calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput() before this module's run() method is called // with module_ptr->run(). Then module_ptr->run() invokes StartPlannerModule::updateData and, @@ -368,6 +371,8 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // prepare poses for preventing check // - current_pose // - estimated_stop_pose (The position assumed when stopped with the minimum stop distance, @@ -397,6 +402,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -646,6 +653,8 @@ bool StartPlannerModule::canTransitSuccessState() BehaviorModuleOutput StartPlannerModule::plan() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); @@ -662,6 +671,8 @@ BehaviorModuleOutput StartPlannerModule::plan() } const auto path = std::invoke([&]() { + universe_utils::ScopedTimeTrack st2("plan path", *time_keeper_); + if (!status_.driving_forward && !status_.backward_driving_complete) { return status_.backward_path; } @@ -775,6 +786,8 @@ PathWithLaneId StartPlannerModule::getFullPath() const BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + updatePullOutStatus(); if (!status_.found_pull_out_path) { @@ -868,6 +881,8 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, const Pose & goal_pose, const std::string & search_priority) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (start_pose_candidates.empty()) return; auto get_accumulated_debug_stream = [](const std::vector & debug_data_vector) { @@ -884,18 +899,22 @@ void StartPlannerModule::planWithPriority( determinePriorityOrder(search_priority, start_pose_candidates.size()); std::vector debug_data_vector; - for (const auto & collision_check_margin : parameters_->collision_check_margins) { - for (const auto & [index, planner] : order_priority) { - if (findPullOutPath( - start_pose_candidates[index], planner, refined_start_pose, goal_pose, - collision_check_margin, debug_data_vector)) { - debug_data_.selected_start_pose_candidate_index = index; - debug_data_.margin_for_start_pose_candidate = collision_check_margin; - if (parameters_->print_debug_info) { - const auto ss = get_accumulated_debug_stream(debug_data_vector); - DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); + { // create a scope for the scoped time track + universe_utils::ScopedTimeTrack st2("findPullOutPaths", *time_keeper_); + + for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath( + start_pose_candidates[index], planner, refined_start_pose, goal_pose, + collision_check_margin, debug_data_vector)) { + debug_data_.selected_start_pose_candidate_index = index; + debug_data_.margin_for_start_pose_candidate = collision_check_margin; + if (parameters_->print_debug_info) { + const auto ss = get_accumulated_debug_stream(debug_data_vector); + DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); + } + return; } - return; } } } @@ -910,6 +929,8 @@ void StartPlannerModule::planWithPriority( PriorityOrder StartPlannerModule::determinePriorityOrder( const std::string & search_priority, const size_t start_pose_candidates_num) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + PriorityOrder order_priority; if (search_priority == "efficient_path") { for (const auto & planner : start_planners_) { @@ -1080,6 +1101,8 @@ std::vector StartPlannerModule::generateDrivableLanes( void StartPlannerModule::updatePullOutStatus() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // skip updating if enough time has not passed for preventing chattering between back and // start_planner if (!receivedNewRoute()) { @@ -1148,6 +1171,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -1175,6 +1200,8 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const std::vector StartPlannerModule::searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::vector pull_out_start_pose_candidates{}; const auto start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto local_vehicle_footprint = vehicle_info_.createFootprint(); @@ -1239,6 +1266,8 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( const double velocity_threshold, const double object_check_forward_distance, const double object_check_backward_distance) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); @@ -1388,6 +1417,8 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() bool StartPlannerModule::isSafePath() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // TODO(Sugahara): should safety check for backward path const auto pull_out_path = getCurrentPath();