From ab7d208bc9d44b9be9a7562689edf2f22be105ab Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 10 Jun 2023 01:17:41 +0900 Subject: [PATCH] refactoring Signed-off-by: kyoichi-sugahara --- .../utils/path_utils.hpp | 8 +++++--- .../src/marker_util/debug_utilities.cpp | 1 + .../start_planner/start_planner_module.cpp | 16 +++++++++++---- .../src/utils/path_utils.cpp | 20 ++++++++++--------- 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index 2dd4e05ea35f0..a4c20df9b324d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -59,10 +59,12 @@ PathWithLaneId resamplePathWithSpline( const PathWithLaneId & path, const double interval, const bool keep_input_points = false, const std::pair target_section = {0.0, std::numeric_limits::max()}); +// TODO(Sugahara): Add explanation and unit test PredictedPath createPredictedPathFromTargetVelocity( - const PathWithLaneId & following_trajectory, const double current_velocity, - const double target_velocity, const double acc_till_target_velocity, const Pose & pose, - const size_t nearest_seg_idx, const double resolution, const double stopping_time); + const std::vector & following_trajectory_points, + const double current_velocity, const double target_velocity, + const double acc_till_target_velocity, const Pose & pose, const size_t nearest_seg_idx, + const double resolution, const double stopping_time); Path toPath(const PathWithLaneId & input); diff --git a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp b/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp index 4af733926dfa6..313604184f52f 100644 --- a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp +++ b/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp @@ -209,6 +209,7 @@ MarkerArray createEgoPredictedPathMarkerArray( const auto & path = ego_predicted_path.path; + // TODO(Sugahara): create foot print marker and change the color Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(r, g, b, 0.999)); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 3def4e5592c31..efe3d7ee494d7 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -824,17 +824,25 @@ void StartPlannerModule::setDebugData() const const auto add = [this](const MarkerArray & added) { tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; + + // TODO(Sugahara): define param for target_velocity, acc_till_target_velocity, resolution, + // stopping_time size_t nearest_segment_index = motion_utils::findNearestIndex( status_.pull_out_path.partial_paths.back().points, planner_data_->self_odometry->pose.pose.position); + const double current_velocity = planner_data_->self_odometry->twist.twist.linear.x; + const double target_velocity = 2.0; + const double acc_till_target_velocity = 0.5; + const Pose current_pose = planner_data_->self_odometry->pose.pose; + const double resolution = 0.5; + const double stopping_time = 1.0; const auto & ego_predicted_path = utils::createPredictedPathFromTargetVelocity( - status_.pull_out_path.partial_paths.back(), planner_data_->self_odometry->twist.twist.linear.x, - 2.0, 0.5, planner_data_->self_odometry->pose.pose, nearest_segment_index, 0.5, 1.0); + status_.pull_out_path.partial_paths.back().points, current_velocity, target_velocity, + acc_till_target_velocity, current_pose, nearest_segment_index, resolution, stopping_time); debug_marker_.markers.clear(); - add(createEgoPredictedPathMarkerArray( - ego_predicted_path, "ego_predicted_path_start_planner", 0.9, 0.3, 0.3)); + add(createEgoPredictedPathMarkerArray(ego_predicted_path, "ego_predicted_path", 0.9, 0.3, 0.3)); add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index ecdd6c602a6f3..212aacb2c7ce5 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -127,21 +127,22 @@ PathWithLaneId resamplePathWithSpline( } PredictedPath createPredictedPathFromTargetVelocity( - const PathWithLaneId & following_trajectory, const double current_velocity, - const double target_velocity, const double acc_till_target_velocity, const Pose & pose, - const size_t nearest_seg_idx, const double resolution, const double stopping_time) + const std::vector & following_trajectory_points, + const double current_velocity, const double target_velocity, + const double acc_till_target_velocity, const Pose & pose, const size_t nearest_seg_idx, + const double resolution, const double stopping_time) { PredictedPath predicted_path{}; predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); predicted_path.path.reserve( - std::min(following_trajectory.points.size(), static_cast(100))); + std::min(following_trajectory_points.size(), static_cast(100))); - if (following_trajectory.points.empty()) { + if (following_trajectory_points.empty()) { return predicted_path; } FrenetPoint vehicle_pose_frenet = - convertToFrenetPoint(following_trajectory.points, pose.position, nearest_seg_idx); + convertToFrenetPoint(following_trajectory_points, pose.position, nearest_seg_idx); // add a segment to the path auto addSegment = [&]( @@ -164,22 +165,23 @@ PredictedPath createPredictedPathFromTargetVelocity( acc_time = 0.0; } + // TODO(Sugahara): Stopping time is considered only when the vehicle is stopping. // Stopping segment, only if stopping_time is greater than zero if (stopping_time > 0.0) { - addSegment(following_trajectory.points, predicted_path, 0.0, 0.0, 0.0, stopping_time, 0.0); + addSegment(following_trajectory_points, predicted_path, 0.0, 0.0, 0.0, stopping_time, 0.0); } // Acceleration segment double offset = 0.0; addSegment( - following_trajectory.points, predicted_path, current_velocity, acc_till_target_velocity, + following_trajectory_points, predicted_path, current_velocity, acc_till_target_velocity, stopping_time, stopping_time + acc_time, offset); // Constant velocity segment offset += current_velocity * acc_time + 0.5 * acc_till_target_velocity * acc_time * acc_time; double constant_velocity_time = stopping_time + acc_time + (target_velocity / resolution); addSegment( - following_trajectory.points, predicted_path, target_velocity, 0.0, stopping_time + acc_time, + following_trajectory_points, predicted_path, target_velocity, 0.0, stopping_time + acc_time, constant_velocity_time, offset); return predicted_path;