feat(autoware_behavior_velocity_planner_common,autoware_behavior_velo…

…city_stop_line_module): add time_keeper to bvp (autowarefoundation#8070) Signed-off-by: Y.Hisaki <[email protected]> Signed-off-by: chtseng <[email protected]>
chihungtzeng · Jul 23, 2024 · be56a2f · be56a2f
1 parent a2ef703
commit be56a2f
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 2 deletions.
diff --git a/...anner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/...anner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp
@@ -22,6 +22,7 @@
 #include <autoware/rtc_interface/rtc_interface.hpp>
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/ros/parameter.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
@@ -42,6 +43,7 @@
 #include <vector>
 
 // Debug
+#include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -106,6 +108,13 @@ class SceneModuleInterface
     infrastructure_command_ = command;
   }
 
+  void setTimeKeeper(const std::shared_ptr<universe_utils::TimeKeeper> & time_keeper)
+  {
+    time_keeper_ = time_keeper;
+  }
+
+  std::shared_ptr<universe_utils::TimeKeeper> getTimeKeeper() { return time_keeper_; }
+
   std::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
 
   void setActivation(const bool activated) { activated_ = activated; }
@@ -132,6 +141,7 @@ class SceneModuleInterface
   std::optional<int> first_stop_path_point_index_;
   autoware::motion_utils::VelocityFactorInterface velocity_factor_;
   std::vector<ObjectOfInterest> objects_of_interest_;
+  mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
 
   void setSafe(const bool safe)
   {
@@ -215,6 +225,10 @@ class SceneModuleManagerInterface
     pub_infrastructure_commands_;
 
   std::shared_ptr<DebugPublisher> processing_time_publisher_;
+
+  rclcpp::Publisher<universe_utils::ProcessingTimeDetail>::SharedPtr pub_processing_time_detail_;
+
+  std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
 };
 
 class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface

diff --git a/...velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/...velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp
@@ -17,9 +17,11 @@
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 
 #include <algorithm>
 #include <limits>
+#include <memory>
 
 namespace autoware::behavior_velocity_planner
 {
@@ -33,7 +35,8 @@ SceneModuleInterface::SceneModuleInterface(
   safe_(false),
   distance_(std::numeric_limits<double>::lowest()),
   logger_(logger),
-  clock_(clock)
+  clock_(clock),
+  time_keeper_(std::shared_ptr<universe_utils::TimeKeeper>())
 {
 }
 
@@ -71,6 +74,11 @@ SceneModuleManagerInterface::SceneModuleManagerInterface(
       "~/output/infrastructure_commands", 1);
 
   processing_time_publisher_ = std::make_shared<DebugPublisher>(&node, "~/debug");
+
+  pub_processing_time_detail_ = node.create_publisher<universe_utils::ProcessingTimeDetail>(
+    "~/debug/processing_time_detail_ms/" + std::string(module_name), 1);
+
+  time_keeper_ = std::make_shared<universe_utils::TimeKeeper>(pub_processing_time_detail_);
 }
 
 size_t SceneModuleManagerInterface::findEgoSegmentIndex(
@@ -94,6 +102,8 @@ void SceneModuleManagerInterface::updateSceneModuleInstances(
 void SceneModuleManagerInterface::modifyPathVelocity(
   tier4_planning_msgs::msg::PathWithLaneId * path)
 {
+  universe_utils::ScopedTimeTrack st(
+    "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_);
   StopWatch<std::chrono::milliseconds> stop_watch;
   stop_watch.tic("Total");
   visualization_msgs::msg::MarkerArray debug_marker_array;
@@ -177,6 +187,7 @@ void SceneModuleManagerInterface::registerModule(
   RCLCPP_DEBUG(
     logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId());
   registered_module_id_set_.emplace(scene_module->getModuleId());
+  scene_module->setTimeKeeper(time_keeper_);
   scene_modules_.insert(scene_module);
 }
 

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp
@@ -17,6 +17,7 @@
 #include <autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp>
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 
 #include <algorithm>
 #include <vector>
@@ -40,6 +41,8 @@ StopLineModule::StopLineModule(
 
 bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
 {
+  universe_utils::ScopedTimeTrack st(
+    std::string(__func__) + " (lane_id:=" + std::to_string(module_id_) + ")", *getTimeKeeper());
   debug_data_ = DebugData();
   if (path->points.empty()) return true;
   const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
@@ -50,11 +53,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
   const LineString2d stop_line = planning_utils::extendLine(
     stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);
 
+  time_keeper_->start_track("createTargetPoint");
   // Calculate stop pose and insert index
   const auto stop_point = arc_lane_utils::createTargetPoint(
     *path, stop_line, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
-
+  time_keeper_->end_track("createTargetPoint");
   // If no collision found, do nothing
   if (!stop_point) {
     RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision");
@@ -70,12 +74,16 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
    * |----------------------------|
    * s---ego----------x--|--------g
    */
+  time_keeper_->start_track(
+    "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
   const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex(
     path->points, stop_pose.position, stop_point_idx);
   const size_t current_seg_idx = findEgoSegmentIndex(path->points);
   const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength(
     path->points, planner_data_->current_odometry->pose.position, current_seg_idx,
     stop_pose.position, stop_line_seg_idx);
+  time_keeper_->end_track(
+    "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
   switch (state_) {
     case State::APPROACH: {
       // Insert stop pose