feat(start_planner): add time_keeper (#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>

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

@@ -17,6 +17,7 @@
 
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <autoware/universe_utils/geometry/pose_deviation.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rosidl_runtime_cpp/message_initialization.hpp>
 
@@ -101,6 +102,12 @@ struct Output
 class LaneDepartureChecker
 {
 public:
+  LaneDepartureChecker(
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
+      std::make_shared<universe_utils::TimeKeeper>())
+  : time_keeper_(time_keeper)
+  {
+  }
   Output update(const Input & input);
 
   void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info)
@@ -156,19 +163,21 @@ class LaneDepartureChecker
   static std::vector<LinearRing2d> createVehiclePassingAreas(
     const std::vector<LinearRing2d> & vehicle_footprints);
 
-  static bool willLeaveLane(
+  bool willLeaveLane(
     const lanelet::ConstLanelets & candidate_lanelets,
-    const std::vector<LinearRing2d> & vehicle_footprints);
+    const std::vector<LinearRing2d> & vehicle_footprints) const;
 
   double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const;
 
   static SegmentRtree extractUncrossableBoundaries(
     const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point,
     const double max_search_length, const std::vector<std::string> & boundary_types_to_detect);
 
-  static bool willCrossBoundary(
+  bool willCrossBoundary(
     const std::vector<LinearRing2d> & vehicle_footprints,
-    const SegmentRtree & uncrossable_segments);
+    const SegmentRtree & uncrossable_segments) const;
+
+  mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
 };
 }  // namespace autoware::lane_departure_checker
 

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<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
   lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
   return willLeaveLane(candidate_lanelets, vehicle_footprints);
@@ -290,8 +292,10 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
 
 bool LaneDepartureChecker::willLeaveLane(
   const lanelet::ConstLanelets & candidate_lanelets,
-  const std::vector<LinearRing2d> & vehicle_footprints)
+  const std::vector<LinearRing2d> & 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<std::pair<double, lanelet::Lanelet>> 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<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
   LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
@@ -326,6 +332,8 @@ std::optional<autoware::universe_utils::Polygon2d>
 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<LinearRing2d> 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<LinearRing2d> & vehicle_footprints, const SegmentRtree & uncrossable_segments)
+  const std::vector<LinearRing2d> & vehicle_footprints,
+  const SegmentRtree & uncrossable_segments) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   for (const auto & footprint : vehicle_footprints) {
     std::vector<Segment2d> intersection_result;
     uncrossable_segments.query(

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 <autoware/freespace_planning_algorithms/abstract_algorithm.hpp>
 #include <autoware/freespace_planning_algorithms/astar_search.hpp>
@@ -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<universe_utils::TimeKeeper> time_keeper);
 
   PlannerType getPlannerType() const override { return PlannerType::FREESPACE; }
 

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 <autoware/lane_departure_checker/lane_departure_checker.hpp>
 
@@ -33,7 +34,8 @@ class GeometricPullOut : public PullOutPlannerBase
   explicit GeometricPullOut(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
     const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
-      lane_departure_checker);
+      lane_departure_checker,
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
 
   PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; };
   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

@@ -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 <geometry_msgs/msg/pose_stamped.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
@@ -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<universe_utils::TimeKeeper> 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<universe_utils::TimeKeeper> time_keeper_;
 };
 }  // namespace autoware::behavior_path_planner
 

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 <autoware/lane_departure_checker/lane_departure_checker.hpp>
 
@@ -34,7 +35,8 @@ class ShiftPullOut : public PullOutPlannerBase
 public:
   explicit ShiftPullOut(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
-    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
+    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper);
 
   PlannerType getPlannerType() const override { return PlannerType::SHIFT; };
   std::optional<PullOutPath> plan(

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<universe_utils::TimeKeeper> 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);

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<autoware::lane_departure_checker::LaneDepartureChecker>
-    lane_departure_checker)
-: PullOutPlannerBase{node, parameters},
+    lane_departure_checker,
+  std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
+: PullOutPlannerBase{node, parameters, time_keeper},
   parallel_parking_parameters_{parameters.parallel_parking_parameters},
   lane_departure_checker_(lane_departure_checker)
 {

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<LaneDepartureChecker> & lane_departure_checker)
-: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}
+  std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
+  std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
+: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker}
 {
 }
 
@@ -62,6 +63,8 @@ std::optional<PullOutPath> 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<PullOutPath> 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<PullOutPath> candidate_paths{};
 
   if (road_lanes.empty()) {