From 859885e1740fdb6d3a847303364b052471a01069 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 15 Dec 2023 23:45:24 +0900 Subject: [PATCH] perf(run_out): improve calculation cost of smoothPath (#5885) * perf(run_out): improve calculation cost of smoothPath Signed-off-by: Takayuki Murooka * re-index enum elements Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/debug.hpp | 19 +++--- .../src/scene.cpp | 68 ++++++++++++------- 2 files changed, 54 insertions(+), 33 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 273abb10c780a..0a2a92f3055fb 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -33,14 +33,17 @@ class DebugValues public: enum class TYPE { CALCULATION_TIME = 0, - CALCULATION_TIME_COLLISION_CHECK = 1, - LATERAL_DIST = 2, - LONGITUDINAL_DIST_OBSTACLE = 3, - LONGITUDINAL_DIST_COLLISION = 4, - COLLISION_POS_FROM_EGO_FRONT = 5, - STOP_DISTANCE = 6, - NUM_OBSTACLES = 7, - LATERAL_PASS_DIST = 8, + CALCULATION_TIME_PATH_PROCESSING = 1, + CALCULATION_TIME_OBSTACLE_CREATION = 2, + CALCULATION_TIME_COLLISION_CHECK = 3, + CALCULATION_TIME_PATH_PLANNING = 4, + LATERAL_DIST = 5, + LONGITUDINAL_DIST_OBSTACLE = 6, + LONGITUDINAL_DIST_COLLISION = 7, + COLLISION_POS_FROM_EGO_FRONT = 8, + STOP_DISTANCE = 9, + NUM_OBSTACLES = 10, + LATERAL_PASS_DIST = 11, SIZE, // this is the number of enum elements }; diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index a943e37d666db..0cf7714cbcaee 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -55,7 +55,7 @@ bool RunOutModule::modifyPathVelocity( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { // timer starts - const auto t1_modify_path = std::chrono::system_clock::now(); + const auto t_start = std::chrono::system_clock::now(); // set planner data const auto current_vel = planner_data_->current_velocity->twist.linear.x; @@ -65,20 +65,27 @@ bool RunOutModule::modifyPathVelocity( // set height of debug data debug_ptr_->setHeight(current_pose.position.z); - // smooth velocity of the path to calculate time to collision accurately - PathWithLaneId smoothed_path; - if (!smoothPath(*path, smoothed_path, planner_data_)) { - return true; - } - // extend path to consider obstacles after the goal - const auto extended_smoothed_path = - run_out_utils::extendPath(smoothed_path, planner_param_.vehicle_param.base_to_front); + const auto extended_path = + run_out_utils::extendPath(*path, planner_param_.vehicle_param.base_to_front); // trim path ahead of the base_link to make calculation easier const double trim_distance = planner_param_.run_out.detection_distance; - const auto trim_smoothed_path = - run_out_utils::trimPathFromSelfPose(extended_smoothed_path, current_pose, trim_distance); + const auto trim_path = + run_out_utils::trimPathFromSelfPose(extended_path, current_pose, trim_distance); + + // smooth velocity of the path to calculate time to collision accurately + PathWithLaneId extended_smoothed_path; + if (!smoothPath(trim_path, extended_smoothed_path, planner_data_)) { + return true; + } + + // record time for path processing + const auto t_path_processing = std::chrono::system_clock::now(); + const auto elapsed_path_processing = + std::chrono::duration_cast(t_path_processing - t_start); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_PATH_PROCESSING, elapsed_path_processing.count() / 1000.0); // create abstracted dynamic obstacles from objects or points dynamic_obstacle_creator_->setData(*planner_data_, planner_param_, *path, extended_smoothed_path); @@ -87,18 +94,24 @@ bool RunOutModule::modifyPathVelocity( // extract obstacles using lanelet information const auto partition_excluded_obstacles = - excludeObstaclesOutSideOfPartition(dynamic_obstacles, trim_smoothed_path, current_pose); + excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); - // timer starts - const auto t1_collision_check = std::chrono::system_clock::now(); + // record time for obstacle creation + const auto t_obstacle_creation = std::chrono::system_clock::now(); + const auto elapsed_obstacle_creation = + std::chrono::duration_cast(t_obstacle_creation - t_path_processing); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_OBSTACLE_CREATION, + elapsed_obstacle_creation.count() / 1000.0); // detect collision with dynamic obstacles using velocity planning of ego - const auto dynamic_obstacle = detectCollision(partition_excluded_obstacles, trim_smoothed_path); + const auto dynamic_obstacle = + detectCollision(partition_excluded_obstacles, extended_smoothed_path); - // timer ends - const auto t2_collision_check = std::chrono::system_clock::now(); + // record time for collision check + const auto t_collision_check = std::chrono::system_clock::now(); const auto elapsed_collision_check = - std::chrono::duration_cast(t2_collision_check - t1_collision_check); + std::chrono::duration_cast(t_collision_check - t_obstacle_creation); debug_ptr_->setDebugValues( DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0); @@ -107,7 +120,7 @@ bool RunOutModule::modifyPathVelocity( // after a certain amount of time has elapsed since the ego stopped, // approach the obstacle with slow velocity insertVelocityForState( - dynamic_obstacle, *planner_data_, planner_param_, trim_smoothed_path, *path); + dynamic_obstacle, *planner_data_, planner_param_, extended_smoothed_path, *path); } else { // just insert zero velocity for stopping insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); @@ -119,14 +132,19 @@ bool RunOutModule::modifyPathVelocity( } publishDebugValue( - trim_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); + extended_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); - // timer ends - const auto t2_modify_path = std::chrono::system_clock::now(); - const auto elapsed_modify_path = - std::chrono::duration_cast(t2_modify_path - t1_modify_path); + // record time for collision check + const auto t_path_planning = std::chrono::system_clock::now(); + const auto elapsed_path_planning = + std::chrono::duration_cast(t_path_planning - t_collision_check); debug_ptr_->setDebugValues( - DebugValues::TYPE::CALCULATION_TIME, elapsed_modify_path.count() / 1000.0); + DebugValues::TYPE::CALCULATION_TIME_PATH_PLANNING, elapsed_path_planning.count() / 1000.0); + + // record time for the function + const auto t_end = std::chrono::system_clock::now(); + const auto elapsed_all = std::chrono::duration_cast(t_end - t_start); + debug_ptr_->setDebugValues(DebugValues::TYPE::CALCULATION_TIME, elapsed_all.count() / 1000.0); return true; }