feat(autoware_behavior_path_planner_common,autoware_behavior_path_lane_change_module): add time_keeper to bpp (autowarefoundation#8004)

* feat(autoware_behavior_path_planner_common,autoware_behavior_path_lane_change_module): add time_keeper to bpp

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* update

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

---------

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Author: yhisaki

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp

@@ -23,6 +23,7 @@
 #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
@@ -108,6 +109,7 @@ class LaneChangeInterface : public SceneModuleInterface
     const double start_distance, const double finish_distance, const bool safe,
     const uint8_t & state)
   {
+    universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
       if (ptr) {
         ptr->updateCooperateStatus(

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp

@@ -23,6 +23,7 @@
 #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 #include "autoware/universe_utils/system/stop_watch.hpp"
 
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <magic_enum.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -53,7 +54,8 @@ class LaneChangeBase
   : lane_change_parameters_{std::move(parameters)},
     common_data_ptr_{std::make_shared<lane_change::CommonData>()},
     direction_{direction},
-    type_{type}
+    type_{type},
+    time_keeper_(std::make_shared<universe_utils::TimeKeeper>())
   {
   }
 
@@ -167,6 +169,11 @@ class LaneChangeBase
     common_data_ptr_->direction = direction_;
   }
 
+  void setTimeKeeper(const std::shared_ptr<universe_utils::TimeKeeper> & time_keeper)
+  {
+    time_keeper_ = time_keeper;
+  }
+
   void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }
 
   void toStopState() { current_lane_change_state_ = LaneChangeStates::Stop; }
@@ -252,6 +259,8 @@ class LaneChangeBase
 
   rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr());
   mutable rclcpp::Clock clock_{RCL_ROS_TIME};
+
+  mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
 };
 }  // namespace autoware::behavior_path_planner
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -21,6 +21,7 @@
 #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp"
 
 #include <autoware/universe_utils/ros/marker_helper.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 
 #include <algorithm>
 #include <memory>
@@ -43,6 +44,7 @@ LaneChangeInterface::LaneChangeInterface(
   module_type_{std::move(module_type)},
   prev_approved_path_{std::make_unique<PathWithLaneId>()}
 {
+  module_type_->setTimeKeeper(getTimeKeeper());
   steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, name);
   logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr());
 }
@@ -71,6 +73,7 @@ bool LaneChangeInterface::isExecutionReady() const
 
 void LaneChangeInterface::updateData()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
   module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
   module_type_->updateSpecialData();
 
@@ -94,6 +97,7 @@ void LaneChangeInterface::postProcess()
 
 BehaviorModuleOutput LaneChangeInterface::plan()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
   resetPathCandidate();
   resetPathReference();
 
@@ -165,6 +169,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
 CandidateOutput LaneChangeInterface::planCandidate() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
   const auto selected_path = module_type_->getLaneChangePath();
 
   if (selected_path.path.points.empty()) {
@@ -340,6 +345,7 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall()
 
 void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
   const auto steering_factor_direction = std::invoke([&]() {
     if (module_type_->getDirection() == Direction::LEFT) {
       return SteeringFactor::LEFT;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -23,6 +23,7 @@
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 
 #include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 
@@ -55,6 +56,7 @@ NormalLaneChange::NormalLaneChange(
 
 void NormalLaneChange::updateLaneChangeStatus()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   updateStopTime();
   const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path);
 
@@ -263,6 +265,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
 
 BehaviorModuleOutput NormalLaneChange::generateOutput()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   if (!status_.is_valid_path) {
     RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output.");
     return prev_module_output_;
@@ -308,6 +311,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
 
 void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto & dp = planner_data_->drivable_area_expansion_parameters;
 
   const auto drivable_lanes = utils::lane_change::generateDrivableLanes(
@@ -327,6 +331,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c
 void NormalLaneChange::insertStopPoint(
   const lanelet::ConstLanelets & lanelets, PathWithLaneId & path)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   if (lanelets.empty()) {
     return;
   }
@@ -468,6 +473,7 @@ void NormalLaneChange::insertStopPoint(
 
 PathWithLaneId NormalLaneChange::getReferencePath() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   lanelet::ConstLanelet closest_lanelet;
   if (!lanelet::utils::query::getClosestLanelet(
         status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) {
@@ -482,6 +488,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const
 
 std::optional<PathWithLaneId> NormalLaneChange::extendPath()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto path = status_.lane_change_path.path;
   const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position;
 
@@ -560,6 +567,7 @@ void NormalLaneChange::resetParameters()
 
 TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto & pose = getEgoPose();
   const auto & current_lanes = status_.current_lanes;
   const auto & shift_line = status_.lane_change_path.info.shift_line;
@@ -584,6 +592,7 @@ lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
 lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes(
   const lanelet::ConstLanelets & current_lanes, Direction direction) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   if (current_lanes.empty()) {
     return {};
   }
@@ -1832,6 +1841,7 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis(
 
 bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto & route_handler = planner_data_->route_handler;
   const auto & dp = planner_data_->drivable_area_expansion_parameters;
 
@@ -1869,6 +1879,7 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const
 
 bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear)
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane;
   if (
     isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) &&
@@ -1882,6 +1893,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear)
 
 bool NormalLaneChange::calcAbortPath()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto & route_handler = getRouteHandler();
   const auto & common_param = getCommonParam();
   const auto current_velocity =
@@ -2025,6 +2037,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
   const utils::path_safety_checker::RSSparams & rss_params,
   CollisionCheckDebugMap & debug_data) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   PathSafetyStatus path_safety_status;
 
   if (collision_check_objects.empty()) {
@@ -2108,6 +2121,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 bool NormalLaneChange::isVehicleStuck(
   const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   // Ego is still moving, not in stuck
   if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) {
     RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck");
@@ -2163,6 +2177,7 @@ bool NormalLaneChange::isVehicleStuck(
 
 double NormalLaneChange::get_max_velocity_for_safety_check() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity;
   if (external_velocity_limit_ptr) {
     return std::min(
@@ -2174,6 +2189,7 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const
 
 bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   if (current_lanes.empty()) {
     lane_change_debug_.is_stuck = false;
     return false;  // can not check
@@ -2208,6 +2224,7 @@ void NormalLaneChange::setStopPose(const Pose & stop_pose)
 
 void NormalLaneChange::updateStopTime()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto current_vel = getEgoVelocity();
 
   if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) {

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp

@@ -21,6 +21,7 @@
 #include "autoware/universe_utils/ros/debug_publisher.hpp"
 #include "autoware/universe_utils/system/stop_watch.hpp"
 
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -277,6 +278,7 @@ class PlannerManager
     const SceneModulePtr & module_ptr, const std::shared_ptr<PlannerData> & planner_data,
     const BehaviorModuleOutput & previous_module_output) const
   {
+    universe_utils::ScopedTimeTrack st(module_ptr->name() + "=>run", *module_ptr->getTimeKeeper());
     stop_watch_.tic(module_ptr->name());
 
     module_ptr->setData(planner_data);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -31,6 +31,7 @@
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <magic_enum.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -97,7 +98,8 @@ class SceneModuleInterface
     objects_of_interest_marker_interface_ptr_map_(
       std::move(objects_of_interest_marker_interface_ptr_map)),
     steering_factor_interface_ptr_(
-      std::make_unique<SteeringFactorInterface>(&node, utils::convertToSnakeCase(name)))
+      std::make_unique<SteeringFactorInterface>(&node, utils::convertToSnakeCase(name))),
+    time_keeper_(std::make_shared<universe_utils::TimeKeeper>())
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
       uuid_map_.emplace(module_name, generateUUID());
@@ -244,6 +246,8 @@ class SceneModuleInterface
     previous_module_output_ = previous_module_output;
   }
 
+  std::shared_ptr<universe_utils::TimeKeeper> getTimeKeeper() const { return time_keeper_; }
+
   /**
    * @brief set planner data
    */
@@ -641,6 +645,8 @@ class SceneModuleInterface
   mutable MarkerArray debug_marker_;
 
   mutable MarkerArray drivable_lanes_marker_;
+
+  mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
 };
 
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp

@@ -85,6 +85,7 @@ class SceneModuleManagerInterface
 
     observer.lock()->setData(planner_data_);
     observer.lock()->setPreviousModuleOutput(previous_module_output);
+    observer.lock()->getTimeKeeper()->add_reporter(this->pub_processing_time_);
     observer.lock()->onEntry();
 
     observers_.push_back(observer);
@@ -318,6 +319,8 @@ class SceneModuleManagerInterface
       pub_debug_marker_ = node->create_publisher<MarkerArray>("~/debug/" + name_, 20);
       pub_virtual_wall_ = node->create_publisher<MarkerArray>("~/virtual_wall/" + name_, 20);
       pub_drivable_lanes_ = node->create_publisher<MarkerArray>("~/drivable_lanes/" + name_, 20);
+      pub_processing_time_ = node->create_publisher<universe_utils::ProcessingTimeDetail>(
+        "~/processing_time/" + name_, 20);
     }
 
     // misc
@@ -338,6 +341,8 @@ class SceneModuleManagerInterface
 
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_drivable_lanes_;
 
+  rclcpp::Publisher<universe_utils::ProcessingTimeDetail>::SharedPtr pub_processing_time_;
+
   std::string name_;
 
   std::shared_ptr<PlannerData> planner_data_;