From a09b2c985d82674fb61b930e695b671bdcbbce9b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 9 Jun 2023 16:12:05 +0900 Subject: [PATCH 1/8] fix(map_loader): handle enable_selected_load correctly (#3920) * fix(map_loader): update readme for metadata Signed-off-by: kminoda * fix(map_loader): handle enable_selected_load flag correctly Signed-off-by: kminoda * style(pre-commit): autofix * revert readme Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/pointcloud_map_loader/pointcloud_map_loader_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index dda25987629ce..a2d9307084545 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -68,7 +68,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load || enable_differential_load) { + if (enable_partial_load || enable_differential_load || enable_selected_load) { std::map pcd_metadata_dict; try { pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); From 21b64038779a2a929b9e409fb6bb13c2392ff789 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 9 Jun 2023 17:35:28 +0900 Subject: [PATCH 2/8] chore(obstacle_cruise_planner): change cruise color (#3925) Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/src/node.cpp | 2 +- .../src/pid_based_planner/pid_based_planner.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 028c82047ed3a..bf705759e0422 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1251,7 +1251,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) { // obstacle const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( - debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.3, 0.0); + debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.5, 0.5); debug_marker.markers.push_back(obstacle_marker); // collision points diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index e729b97ac1deb..bca5ad3f3cf41 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -314,7 +314,7 @@ std::vector PIDBasedPlanner::planCruise( stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0); // NOTE: use a different color from slow down one to visualize cruise and slow down // separately. - markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.3, 0.0, 0.5); + markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.5, 0.5); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); // cruise obstacle From fc08b4f11770a6a2ac137ec8121c20f51179ca68 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 9 Jun 2023 20:33:00 +0900 Subject: [PATCH 3/8] chore(dynamic_avoidance): change debug marker's color (#3926) Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance/dynamic_avoidance_module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 47c29be29c2a3..7ece399bf450c 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -101,7 +101,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8)); marker.pose = obj_pose; marker_array.markers.push_back(marker); @@ -113,8 +113,8 @@ void appendExtractedPolygonMarker( auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. for (size_t i = 0; i < obj_poly.outer().size(); ++i) { From 719a843fc63ff80ffff68d76bc863032857b6d36 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 10 Jun 2023 01:22:26 +0900 Subject: [PATCH 4/8] feat(obstacle_avoidance_planner): consider curvature for min_drivable_width (#3918) * feat(obstacle_avoidance_planner): consider curvature for min_drivable_width Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/mpt_optimizer.cpp | 45 ++++++++++++++----- 1 file changed, 34 insertions(+), 11 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 439b68e9032dd..eff2a653f39e2 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -842,6 +842,28 @@ void MPTOptimizer::updateBounds( void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_points) const { + // calculate drivable area width considering the curvature + std::vector min_dynamic_drivable_width_vec; + for (int i = 0; i < static_cast(ref_points.size()); ++i) { + double curvature = std::abs(ref_points.at(i).curvature); + if (i != static_cast(ref_points.size()) - 1) { + curvature = std::max(curvature, std::abs(ref_points.at(i + 1).curvature)); + } + if (i != 0) { + curvature = std::max(curvature, std::abs(ref_points.at(i - 1).curvature)); + } + + const double max_longitudinal_length = std::max( + std::abs(vehicle_info_.max_longitudinal_offset_m), + std::abs(vehicle_info_.min_longitudinal_offset_m)); + const double turning_radius = 1.0 / curvature; + const double additional_drivable_width_by_curvature = + std::hypot(max_longitudinal_length, turning_radius + vehicle_info_.vehicle_width_m / 2.0) - + turning_radius - vehicle_info_.vehicle_width_m / 2.0; + min_dynamic_drivable_width_vec.push_back( + mpt_param_.min_drivable_width + additional_drivable_width_by_curvature); + } + // 1. calculate start and end sections which are out of bounds std::vector> out_of_upper_bound_sections; std::vector> out_of_lower_bound_sections; @@ -849,8 +871,9 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin std::optional out_of_lower_bound_start_idx = std::nullopt; for (size_t i = 0; i < ref_points.size(); ++i) { const auto & b = ref_points.at(i).bounds; + // const double drivable_width = b.upper_bound - b.lower_bound; - // const bool is_infeasible_to_drive = drivable_width < mpt_param_.min_drivable_width; + // const bool is_infeasible_to_drive = drivable_width < min_dynamic_drivable_width // NOTE: The following condition should be uncommented to see obstacles outside the path. // However, on a narrow road, the ego may go outside the road border with this condition. @@ -912,16 +935,16 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // It seems both bounds are cut out. Widen the bounds towards the both side. const double center_dist_to_bounds = (original_b.upper_bound + original_b.lower_bound) / 2.0; - b.upper_bound = - std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0); - b.lower_bound = - std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0); + b.upper_bound = std::max( + b.upper_bound, center_dist_to_bounds + min_dynamic_drivable_width_vec.at(p_idx) / 2.0); + b.lower_bound = std::min( + b.lower_bound, center_dist_to_bounds - min_dynamic_drivable_width_vec.at(p_idx) / 2.0); continue; } // Only the Lower bound is cut out. Widen the bounds towards the lower bound since cut out too // much. b.lower_bound = - std::min(b.lower_bound, original_b.upper_bound - mpt_param_.min_drivable_width); + std::min(b.lower_bound, original_b.upper_bound - min_dynamic_drivable_width_vec.at(p_idx)); continue; } // extend longitudinal if it overlaps out_of_upper_bound_sections @@ -962,16 +985,16 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // It seems both bounds are cut out. Widen the bounds towards the both side. const double center_dist_to_bounds = (original_b.upper_bound + original_b.lower_bound) / 2.0; - b.upper_bound = - std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0); - b.lower_bound = - std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0); + b.upper_bound = std::max( + b.upper_bound, center_dist_to_bounds + min_dynamic_drivable_width_vec.at(p_idx) / 2.0); + b.lower_bound = std::min( + b.lower_bound, center_dist_to_bounds - min_dynamic_drivable_width_vec.at(p_idx) / 2.0); continue; } // Only the Upper bound is cut out. Widen the bounds towards the upper bound since cut out too // much. b.upper_bound = - std::max(b.upper_bound, original_b.lower_bound + mpt_param_.min_drivable_width); + std::max(b.upper_bound, original_b.lower_bound + min_dynamic_drivable_width_vec.at(p_idx)); continue; } // extend longitudinal if it overlaps out_of_lower_bound_sections From e9f2f4fb4bd1b1dc105d5fd3964565028d426c1b Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Sat, 10 Jun 2023 10:23:54 +0300 Subject: [PATCH 5/8] docs(mission_planner): remove duplicated param from readme (#3933) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- planning/mission_planner/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index e1714420a45f0..4eb91e423c01f 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -22,7 +22,6 @@ In current Autoware.universe, only Lanelet2 map format is supported. | `arrival_check_duration` | double | Duration threshold for goal check | | `goal_angle_threshold` | double | Max goal pose angle for goal approve | | `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | -| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | | `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | | `minimum_reroute_length` | double | Minimum Length for publishing a new route | From 6e7ff50c5185d7f607146a9d9337ced914d066d0 Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Sun, 11 Jun 2023 15:15:45 +0900 Subject: [PATCH 6/8] fix(image_projection_based_fusion): fix type of roi_probability_threshold (#3935) --- .../src/roi_detected_object_fusion/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 3781714d3cf71..37637f99f69c9 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -28,7 +28,7 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio fusion_params_.passthrough_lower_bound_probability_threshold = declare_parameter("passthrough_lower_bound_probability_threshold"); fusion_params_.use_roi_probability = declare_parameter("use_roi_probability"); - fusion_params_.roi_probability_threshold = declare_parameter("roi_probability_threshold"); + fusion_params_.roi_probability_threshold = declare_parameter("roi_probability_threshold"); fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); } From 2d7fbbfd03220426c716183b2fab14040038c67d Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 11 Jun 2023 20:26:41 +0900 Subject: [PATCH 7/8] refactor(velocity_smoother): simplify calcInitialMotion function (#3937) * use initialize smoother Signed-off-by: Takamasa Horibe * refactor initialMotion Signed-off-by: Takamasa Horibe * use member clock Signed-off-by: Takamasa Horibe * fix clang-format Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../motion_velocity_smoother_node.hpp | 7 +- .../smoother/smoother_base.hpp | 2 + .../src/motion_velocity_smoother_node.cpp | 111 ++++++++---------- .../src/smoother/smoother_base.cpp | 5 + 4 files changed, 65 insertions(+), 60 deletions(-) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index bb656a30f3a4d..20cc0ba1957c1 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -63,6 +63,9 @@ struct Motion { double vel = 0.0; double acc = 0.0; + + Motion() {} + Motion(const double v, const double a) : vel(v), acc(a) {} }; class MotionVelocitySmootherNode : public rclcpp::Node @@ -144,7 +147,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node double over_stop_velocity_warn_thr_; // threshold to publish over velocity warn - rclcpp::Clock::SharedPtr clock_; + mutable rclcpp::Clock::SharedPtr clock_; + + void setupSmoother(const double wheelbase); // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index ea85f8dc78615..a7416faa3c652 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -85,6 +85,8 @@ class SmootherBase double getMaxJerk() const; double getMinJerk() const; + void setWheelBase(const double wheel_base); + void setParam(const BaseParam & param); BaseParam getBaseParam() const; diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 357f5a9ada3d8..23053335ac924 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -45,40 +45,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); // create smoother - switch (node_param_.algorithm_type) { - case AlgorithmType::JERK_FILTERED: { - smoother_ = std::make_shared(*this); - - // Set Publisher for jerk filtered algorithm - pub_forward_filtered_trajectory_ = - create_publisher("~/debug/forward_filtered_trajectory", 1); - pub_backward_filtered_trajectory_ = - create_publisher("~/debug/backward_filtered_trajectory", 1); - pub_merged_filtered_trajectory_ = - create_publisher("~/debug/merged_filtered_trajectory", 1); - pub_closest_merged_velocity_ = - create_publisher("~/closest_merged_velocity", 1); - break; - } - case AlgorithmType::L2: { - smoother_ = std::make_shared(*this); - break; - } - case AlgorithmType::LINF: { - smoother_ = std::make_shared(*this); - break; - } - case AlgorithmType::ANALYTICAL: { - smoother_ = std::make_shared(*this); - break; - } - default: - throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); - } - // Initialize the wheelbase - auto p = smoother_->getBaseParam(); - p.wheel_base = wheelbase_; - smoother_->setParam(p); + setupSmoother(wheelbase_); // publishers, subscribers pub_trajectory_ = create_publisher("~/output/trajectory", 1); @@ -127,6 +94,42 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions clock_ = get_clock(); } +void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) +{ + switch (node_param_.algorithm_type) { + case AlgorithmType::JERK_FILTERED: { + smoother_ = std::make_shared(*this); + + // Set Publisher for jerk filtered algorithm + pub_forward_filtered_trajectory_ = + create_publisher("~/debug/forward_filtered_trajectory", 1); + pub_backward_filtered_trajectory_ = + create_publisher("~/debug/backward_filtered_trajectory", 1); + pub_merged_filtered_trajectory_ = + create_publisher("~/debug/merged_filtered_trajectory", 1); + pub_closest_merged_velocity_ = + create_publisher("~/closest_merged_velocity", 1); + break; + } + case AlgorithmType::L2: { + smoother_ = std::make_shared(*this); + break; + } + case AlgorithmType::LINF: { + smoother_ = std::make_shared(*this); + break; + } + case AlgorithmType::ANALYTICAL: { + smoother_ = std::make_shared(*this); + break; + } + default: + throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + } + + smoother_->setWheelBase(wheelbase); +} + rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter( const std::vector & parameters) { @@ -698,33 +701,28 @@ std::pair MotionVelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { - const double vehicle_speed{std::fabs(current_odometry_ptr_->twist.twist.linear.x)}; - const double target_vel{std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps)}; - - Motion initial_motion; - InitializeType type{}; + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); + const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps); // first time if (!current_closest_point_from_prev_output_) { - initial_motion.vel = vehicle_speed; - initial_motion.acc = 0.0; - type = InitializeType::INIT; - return std::make_pair(initial_motion, type); + Motion initial_motion = {vehicle_speed, 0.0}; + return {initial_motion, InitializeType::INIT}; } // when velocity tracking deviation is large - const double desired_vel{current_closest_point_from_prev_output_->longitudinal_velocity_mps}; - const double vel_error{vehicle_speed - std::fabs(desired_vel)}; + const double desired_vel = current_closest_point_from_prev_output_->longitudinal_velocity_mps; + const double desired_acc = current_closest_point_from_prev_output_->acceleration_mps2; + const double vel_error = vehicle_speed - std::fabs(desired_vel); + if (std::fabs(vel_error) > node_param_.replan_vel_deviation) { - type = InitializeType::LARGE_DEVIATION_REPLAN; - initial_motion.vel = vehicle_speed; // use current vehicle speed - initial_motion.acc = current_closest_point_from_prev_output_->acceleration_mps2; + Motion initial_motion = {vehicle_speed, desired_acc}; // TODO(Horibe): use current acc RCLCPP_DEBUG( get_logger(), "calcInitialMotion : Large deviation error for speed control. Use current speed for " "initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f", desired_vel, vehicle_speed, vel_error, node_param_.replan_vel_deviation); - return std::make_pair(initial_motion, type); + return {initial_motion, InitializeType::LARGE_DEVIATION_REPLAN}; } // if current vehicle velocity is low && base_desired speed is high, @@ -737,37 +735,32 @@ MotionVelocitySmootherNode::calcInitialMotion( input_traj.at(*idx), input_traj.at(input_closest)) : 0.0; if (!idx || stop_dist > node_param_.stop_dist_to_prohibit_engage) { - type = InitializeType::ENGAGING; - initial_motion.vel = node_param_.engage_velocity; - initial_motion.acc = node_param_.engage_acceleration; + Motion initial_motion = {node_param_.engage_velocity, node_param_.engage_acceleration}; RCLCPP_DEBUG( get_logger(), "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", vehicle_speed, target_vel, node_param_.engage_velocity, engage_vel_thr, stop_dist); - return std::make_pair(initial_motion, type); + return {initial_motion, InitializeType::ENGAGING}; } else { RCLCPP_DEBUG( get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); } } else if (target_vel > 0.0) { - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_WARN_THROTTLE( - get_logger(), clock, 3000, + get_logger(), *clock_, 3000, "calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ", target_vel, node_param_.engage_velocity); } } // normal update: use closest in current_closest_point_from_prev_output - type = InitializeType::NORMAL; - initial_motion.vel = current_closest_point_from_prev_output_->longitudinal_velocity_mps; - initial_motion.acc = current_closest_point_from_prev_output_->acceleration_mps2; + Motion initial_motion = {desired_vel, desired_acc}; RCLCPP_DEBUG( get_logger(), "calcInitialMotion : normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f", initial_motion.vel, initial_motion.acc, vehicle_speed, target_vel); - return std::make_pair(initial_motion, type); + return {initial_motion, InitializeType::NORMAL}; } void MotionVelocitySmootherNode::overwriteStopPoint( diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index fdfa0f2514b58..2e58961d54c00 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -58,6 +58,11 @@ SmootherBase::SmootherBase(rclcpp::Node & node) node.declare_parameter("sparse_min_interval_distance"); } +void SmootherBase::setWheelBase(const double wheel_base) +{ + base_param_.wheel_base = wheel_base; +} + void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; From 59b1006a3b9adbaa2c3523c5bf60d1518c30e203 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 12 Jun 2023 08:46:48 +0900 Subject: [PATCH 8/8] refactor(avoidance): move small functions to other files (#3909) Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 55 +--- .../utils/avoidance/helper.hpp | 20 ++ .../utils/avoidance/utils.hpp | 11 + .../avoidance/avoidance_module.cpp | 241 ++++-------------- .../src/utils/avoidance/utils.cpp | 109 ++++++++ 5 files changed, 186 insertions(+), 250 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 32b84c2eff26e..0e09ca4482275 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -57,8 +57,8 @@ class AvoidanceModule : public SceneModuleInterface ModuleStatus updateState() override; ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } - BehaviorModuleOutput plan() override; CandidateOutput planCandidate() const override; + BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -322,20 +322,6 @@ class AvoidanceModule : public SceneModuleInterface // shift line generation - /** - * @brief fill index and longitudinal. - * @param target shift line. - * @return processed shift line. - */ - AvoidLine fillAdditionalInfo(const AvoidLine & shift_line) const; - - /** - * @brief fill index and longitudinal. - * @param target shift lines. - * @return processed shift lines. - */ - AvoidLineArray fillAdditionalInfo(const AvoidLineArray & shift_lines) const; - /** * @brief Calculate the shift points (start/end point, shift length) from the object lateral * and longitudinal positions in the Frenet coordinate. The jerk limit is also considered here. @@ -360,16 +346,6 @@ class AvoidanceModule : public SceneModuleInterface AvoidLineArray applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_points, DebugData & debug) const; - /* - * @brief Combine points A into B. If shift_line of A which has same object_id and - * similar shape is already in B, it will not be added into B. - * @param original shift lines. - * @param new shift lines. - * @return processed shift lines. - */ - AvoidLineArray combineRawShiftLinesWithUniqueCheck( - const AvoidLineArray & base_lines, const AvoidLineArray & added_lines) const; - /* * @brief fill gap between two shift lines. * @param original shift lines. @@ -413,15 +389,6 @@ class AvoidanceModule : public SceneModuleInterface */ AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines) const; - /* - * @brief calculate parent ids. - * @param parent shift lines. - * @param child shift lines. - * @return parent ids. - */ - std::vector calcParentIds( - const AvoidLineArray & parent_candidates, const AvoidLine & child) const; - /* * @brief add return shift line from ego position. * @param shift lines which the return shift is added. @@ -490,26 +457,6 @@ class AvoidanceModule : public SceneModuleInterface */ void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const; - /* - * @brief sort shift line order based on their end longitudinal distance. - * @param target shift lines. - * @param re-calculate shift line start length from previous one's. (optional) - */ - void alignShiftLinesOrder( - AvoidLineArray & shift_lines, const bool recalculate_start_length = true) const; - - /** - * @brief fill index and longitudinal. - * @param target shift line. - */ - void fillAdditionalInfoFromPoint(AvoidLineArray & shift_lines) const; - - /** - * @brief fill index and pose. - * @param target shift line. - */ - void fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_lines) const; - /** * @brief add new shift line to path shifter if the RTC status is activated. * @param new shift lines. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index a1b41d3b7d6c0..c949b200dce80 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -156,6 +156,26 @@ class AvoidanceHelper : std::max(shift_length, getRightShiftBound()); } + void alignShiftLinesOrder(AvoidLineArray & lines, const bool align_shift_length = true) const + { + if (lines.empty()) { + return; + } + + std::sort(lines.begin(), lines.end(), [](auto a, auto b) { + return a.end_longitudinal < b.end_longitudinal; + }); + + if (!align_shift_length) { + return; + } + + lines.front().start_shift_length = getEgoLinearShift(); + for (size_t i = 1; i < lines.size(); ++i) { + lines.at(i).start_shift_length = lines.at(i - 1).end_shift_length; + } + } + AvoidLine getMainShiftLine(const AvoidLineArray & lines) const { const auto itr = diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index d3c77ed8603a2..b5b880459a513 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -48,6 +48,8 @@ ShiftLineArray toShiftLineArray(const AvoidLineArray & avoid_points); std::vector concatParentIds( const std::vector & ids1, const std::vector & ids2); +std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2); + double lerpShiftLengthOnArc(double arc, const AvoidLine & al); void fillLongitudinalAndLengthByClosestEnvelopeFootprint( @@ -110,6 +112,15 @@ double extendToRoadShoulderDistanceWithPolygon( const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, const geometry_msgs::msg::Point & overhang_pos, const lanelet::BasicPoint3d & overhang_basic_pose); + +void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); + +void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLineArray & lines); + +AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line); + +AvoidLineArray combineRawShiftLinesWithUniqueCheck( + const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 824ab7f9dc915..2856a20e26a10 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -681,7 +681,9 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif void AvoidanceModule::updateRegisteredRawShiftLines() { - fillAdditionalInfoFromPoint(registered_raw_shift_lines_); + const auto & data = avoidance_data_; + + utils::avoidance::fillAdditionalInfoFromPoint(data, registered_raw_shift_lines_); AvoidLineArray avoid_lines; @@ -701,7 +703,7 @@ void AvoidanceModule::updateRegisteredRawShiftLines() return std::abs(ego_shift_length - reg_shift_length) > THRESHOLD; }; - const auto ego_idx = avoidance_data_.ego_closest_path_index; + const auto ego_idx = data.ego_closest_path_index; for (const auto & s : registered_raw_shift_lines_) { // invalid @@ -720,7 +722,7 @@ void AvoidanceModule::updateRegisteredRawShiftLines() DEBUG_PRINT( "ego_closest_path_index = %lu, registered_raw_shift_lines_ size: %lu -> %lu", - avoidance_data_.ego_closest_path_index, registered_raw_shift_lines_.size(), avoid_lines.size()); + data.ego_closest_path_index, registered_raw_shift_lines_.size(), avoid_lines.size()); printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_ (before)"); printShiftLines(avoid_lines, "registered_raw_shift_lines_ (after)"); @@ -739,8 +741,8 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( * shift, because it cannot handle the case like "we don't have to avoid * the object anymore". */ - auto total_raw_shift_lines = - combineRawShiftLinesWithUniqueCheck(registered_raw_shift_lines_, current_raw_shift_lines); + auto total_raw_shift_lines = utils::avoidance::combineRawShiftLinesWithUniqueCheck( + registered_raw_shift_lines_, current_raw_shift_lines); printShiftLines(current_raw_shift_lines, "current_raw_shift_lines"); printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines"); @@ -796,11 +798,23 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) const auto old_size = registered_raw_shift_lines_.size(); - const auto future_with_info = fillAdditionalInfo(future); + auto future_with_info = future; + utils::avoidance::fillAdditionalInfoFromPoint(avoidance_data_, future_with_info); printShiftLines(future_with_info, "future_with_info"); printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_"); printShiftLines(current_raw_shift_lines_, "current_raw_shift_lines_"); + // sort by longitudinal + std::sort(future_with_info.begin(), future_with_info.end(), [](auto a, auto b) { + return a.end_longitudinal < b.end_longitudinal; + }); + + // calc relative lateral length + future_with_info.front().start_shift_length = getCurrentBaseShift(); + for (size_t i = 1; i < future_with_info.size(); ++i) { + future_with_info.at(i).start_shift_length = future_with_info.at(i - 1).end_shift_length; + } + const auto isAlreadyRegistered = [this](const auto id) { const auto & r = registered_raw_shift_lines_; return std::any_of(r.begin(), r.end(), [id](const auto & r_sl) { return r_sl.id == id; }); @@ -948,8 +962,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( // The end_margin also has the purpose of preventing the return path from NOT being // triggered at the end point. const auto end_margin = 1.0; - const auto return_remaining_distance = std::max( - avoidance_data_.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0); + const auto return_remaining_distance = + std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0); al_return.start_shift_length = shift_length; al_return.end_shift_length = 0.0; @@ -997,125 +1011,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( debug_avoidance_initializer_for_shift_line_time_ = clock_->now(); } - fillAdditionalInfoFromLongitudinal(avoid_lines); + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, avoid_lines); return avoid_lines; } -AvoidLineArray AvoidanceModule::fillAdditionalInfo(const AvoidLineArray & shift_lines) const -{ - if (shift_lines.empty()) { - return shift_lines; - } - - auto out_points = shift_lines; - - const auto & path = avoidance_data_.reference_path; - const auto arclength = avoidance_data_.arclength_from_ego; - - // calc longitudinal - for (auto & sl : out_points) { - sl.start_idx = findNearestIndex(path.points, sl.start.position); - sl.start_longitudinal = arclength.at(sl.start_idx); - sl.end_idx = findNearestIndex(path.points, sl.end.position); - sl.end_longitudinal = arclength.at(sl.end_idx); - } - - // sort by longitudinal - std::sort(out_points.begin(), out_points.end(), [](auto a, auto b) { - return a.end_longitudinal < b.end_longitudinal; - }); - - // calc relative lateral length - out_points.front().start_shift_length = getCurrentBaseShift(); - for (size_t i = 1; i < shift_lines.size(); ++i) { - out_points.at(i).start_shift_length = shift_lines.at(i - 1).end_shift_length; - } - - return out_points; -} -AvoidLine AvoidanceModule::fillAdditionalInfo(const AvoidLine & shift_line) const -{ - const auto ret = fillAdditionalInfo(AvoidLineArray{shift_line}); - return ret.front(); -} - -void AvoidanceModule::fillAdditionalInfoFromPoint(AvoidLineArray & shift_lines) const -{ - if (shift_lines.empty()) { - return; - } - - const auto & path = avoidance_data_.reference_path; - const auto arclength = utils::calcPathArcLengthArray(path); - const auto dist_path_front_to_ego = - calcSignedArcLength(path.points, 0, avoidance_data_.ego_closest_path_index); - - // calc longitudinal - for (auto & sl : shift_lines) { - sl.start_idx = findNearestIndex(path.points, sl.start.position); - sl.start_longitudinal = arclength.at(sl.start_idx) - dist_path_front_to_ego; - sl.end_idx = findNearestIndex(path.points, sl.end.position); - sl.end_longitudinal = arclength.at(sl.end_idx) - dist_path_front_to_ego; - } -} - -void AvoidanceModule::fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_lines) const -{ - const auto & path = avoidance_data_.reference_path; - const auto arclength = utils::calcPathArcLengthArray(path); - const auto path_front_to_ego = - calcSignedArcLength(path.points, 0, avoidance_data_.ego_closest_path_index); - - for (auto & sl : shift_lines) { - sl.start_idx = utils::avoidance::findPathIndexFromArclength( - arclength, sl.start_longitudinal + path_front_to_ego); - sl.start = path.points.at(sl.start_idx).point.pose; - sl.end_idx = utils::avoidance::findPathIndexFromArclength( - arclength, sl.end_longitudinal + path_front_to_ego); - sl.end = path.points.at(sl.end_idx).point.pose; - } -} - -AvoidLineArray AvoidanceModule::combineRawShiftLinesWithUniqueCheck( - const AvoidLineArray & base_lines, const AvoidLineArray & added_lines) const -{ - // TODO(Horibe) parametrize - const auto isSimilar = [](const AvoidLine & a, const AvoidLine & b) { - using tier4_autoware_utils::calcDistance2d; - if (calcDistance2d(a.start, b.start) > 1.0) { - return false; - } - if (calcDistance2d(a.end, b.end) > 1.0) { - return false; - } - if (std::abs(a.end_shift_length - b.end_shift_length) > 0.5) { - return false; - } - return true; - }; - const auto hasSameObjectId = [](const auto & a, const auto & b) { - return a.object.object.object_id == b.object.object.object_id; - }; - - auto combined = base_lines; // initialized - for (const auto & added_line : added_lines) { - bool skip = false; - - for (const auto & base_line : base_lines) { - if (hasSameObjectId(added_line, base_line) && isSimilar(added_line, base_line)) { - skip = true; - break; - } - } - if (!skip) { - combined.push_back(added_line); - } - } - - return combined; -} - void AvoidanceModule::generateTotalShiftLine( const AvoidLineArray & avoid_lines, ShiftLineData & shift_line_data) const { @@ -1308,7 +1208,7 @@ AvoidLineArray AvoidanceModule::fillShiftLineGap(const AvoidLineArray & input) c AvoidLineArray sorted_input = input; - alignShiftLinesOrder(sorted_input, false); + helper_.alignShiftLinesOrder(sorted_input, false); AvoidLineArray output = sorted_input; @@ -1342,9 +1242,9 @@ AvoidLineArray AvoidanceModule::fillShiftLineGap(const AvoidLineArray & input) c fill_gap(sorted_input.at(i), sorted_input.at(i + 1)); } - fillAdditionalInfoFromLongitudinal(output); + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, output); - alignShiftLinesOrder(output, false); + helper_.alignShiftLinesOrder(output, false); return output; } @@ -1361,11 +1261,11 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( // set parent id for (auto & al : merged_shift_lines) { - al.parent_ids = calcParentIds(raw_shift_lines, al); + al.parent_ids = utils::avoidance::calcParentIds(raw_shift_lines, al); } // sort by distance from ego. - alignShiftLinesOrder(merged_shift_lines); + helper_.alignShiftLinesOrder(merged_shift_lines); // debug visualize { @@ -1408,39 +1308,6 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( return merged_shift_lines; } -std::vector AvoidanceModule::calcParentIds( - const AvoidLineArray & parent_candidates, const AvoidLine & child) const -{ - // Get the ID of the original AP whose transition area overlaps with the given AP, - // and set it to the parent id. - std::set ids; - for (const auto & al : parent_candidates) { - const auto p_s = al.start_longitudinal; - const auto p_e = al.end_longitudinal; - const auto has_overlap = !(p_e < child.start_longitudinal || child.end_longitudinal < p_s); - - if (!has_overlap) { - continue; - } - - // Id the shift is overlapped, insert the shift point. Additionally, the shift which refers - // to the same object id (created by the same object) will be set. - // - // Why? : think that there are two shifts, avoiding and . - // If you register only the avoiding shift, the return-to-center shift will not be generated - // when you get too close to or over the obstacle. The return-shift can be handled with - // addReturnShift(), but it maybe reasonable to register the return-to-center shift for the - // object at the same time as registering the avoidance shift to remove the complexity of the - // addReturnShift(). - for (const auto & al_local : parent_candidates) { - if (al_local.object.object.object_id == al.object.object.object_id) { - ids.insert(al_local.id); - } - } - } - return std::vector(ids.begin(), ids.end()); -} - AvoidLineArray AvoidanceModule::trimShiftLine( const AvoidLineArray & shift_lines, DebugData & debug) const { @@ -1451,7 +1318,7 @@ AvoidLineArray AvoidanceModule::trimShiftLine( AvoidLineArray sl_array_trimmed = shift_lines; // sort shift points from front to back. - alignShiftLinesOrder(sl_array_trimmed); + helper_.alignShiftLinesOrder(sl_array_trimmed); // - Change the shift length to the previous one if the deviation is small. { @@ -1514,29 +1381,6 @@ AvoidLineArray AvoidanceModule::trimShiftLine( return sl_array_trimmed; } -void AvoidanceModule::alignShiftLinesOrder( - AvoidLineArray & shift_lines, const bool recalculate_start_length) const -{ - if (shift_lines.empty()) { - return; - } - - // sort shift points from front to back. - std::sort(shift_lines.begin(), shift_lines.end(), [](auto a, auto b) { - return a.end_longitudinal < b.end_longitudinal; - }); - - // calc relative length - // NOTE: the input shift point must not have conflict range. Otherwise relative - // length value will be broken. - if (recalculate_start_length) { - shift_lines.front().start_shift_length = helper_.getEgoLinearShift(); - for (size_t i = 1; i < shift_lines.size(); ++i) { - shift_lines.at(i).start_shift_length = shift_lines.at(i - 1).end_shift_length; - } - } -} - void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const { if (threshold < 1.0e-5) { @@ -1547,7 +1391,7 @@ void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const doub sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold; } - alignShiftLinesOrder(shift_lines); + helper_.alignShiftLinesOrder(shift_lines); } void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const @@ -1627,7 +1471,7 @@ void AvoidanceModule::trimSimilarGradShiftLine( } } - alignShiftLinesOrder(avoid_lines); + helper_.alignShiftLinesOrder(avoid_lines); DEBUG_PRINT("size %lu -> %lu", input.size(), avoid_lines.size()); } @@ -1743,7 +1587,8 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const utils::avoidance::findPathIndexFromArclength(arclength, sl_next_modified.start_longitudinal); sl_next_modified.start = avoidance_data_.reference_path.points.at(sl_next_modified.start_idx).point.pose; - sl_next_modified.parent_ids = calcParentIds(current_raw_shift_lines_, sl_next_modified); + sl_next_modified.parent_ids = + utils::avoidance::calcParentIds(current_raw_shift_lines_, sl_next_modified); // Straight shift point if (sl_next_modified.start_idx > sl_now.start_idx) { // the case where a straight route exists. @@ -1752,7 +1597,8 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const utils::avoidance::setEndData( sl_now_modified, sl_prev_length, sl_next_modified.start, sl_next_modified.start_idx, sl_next_modified.start_longitudinal); - sl_now_modified.parent_ids = calcParentIds(current_raw_shift_lines_, sl_now_modified); + sl_now_modified.parent_ids = + utils::avoidance::calcParentIds(current_raw_shift_lines_, sl_now_modified); shift_lines.push_back(sl_now_modified); } shift_lines.push_back(sl_next_modified); @@ -1766,7 +1612,7 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const i = next_avoid_idx; // skip shifting until next_avoid_idx. } - alignShiftLinesOrder(shift_lines); + helper_.alignShiftLinesOrder(shift_lines); DEBUG_PRINT("trimMomentaryReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); } @@ -1907,7 +1753,7 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double } } - alignShiftLinesOrder(shift_lines); + helper_.alignShiftLinesOrder(shift_lines); DEBUG_PRINT("trimSharpReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); } @@ -1952,7 +1798,7 @@ void AvoidanceModule::trimTooSharpShift(AvoidLineArray & avoid_lines) const } } - alignShiftLinesOrder(avoid_lines); + helper_.alignShiftLinesOrder(avoid_lines); DEBUG_PRINT("size %lu -> %lu", avoid_lines_orig.size(), avoid_lines.size()); } @@ -1961,6 +1807,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( AvoidLineArray & sl_candidates, AvoidLineArray & current_raw_shift_lines) const { constexpr double ep = 1.0e-3; + const auto & data = avoidance_data_; const bool has_candidate_point = !sl_candidates.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); @@ -1986,20 +1833,22 @@ void AvoidanceModule::addReturnShiftLineFromEgo( { // avoidance points: Yes, shift points: No -> select last avoidance point. if (has_candidate_point && !has_registered_point) { - alignShiftLinesOrder(sl_candidates, false); + helper_.alignShiftLinesOrder(sl_candidates, false); last_sl = sl_candidates.back(); } // avoidance points: No, shift points: Yes -> select last shift point. if (!has_candidate_point && has_registered_point) { - last_sl = fillAdditionalInfo(AvoidLine{path_shifter_.getLastShiftLine().get()}); + last_sl = utils::avoidance::fillAdditionalInfo( + data, AvoidLine{path_shifter_.getLastShiftLine().get()}); } // avoidance points: Yes, shift points: Yes -> select the last one from both. if (has_candidate_point && has_registered_point) { - alignShiftLinesOrder(sl_candidates, false); + helper_.alignShiftLinesOrder(sl_candidates, false); const auto & al = sl_candidates.back(); - const auto & sl = fillAdditionalInfo(AvoidLine{path_shifter_.getLastShiftLine().get()}); + const auto & sl = utils::avoidance::fillAdditionalInfo( + data, AvoidLine{path_shifter_.getLastShiftLine().get()}); last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d01d586a0bb11..fd7ba96b89762 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -193,6 +193,38 @@ std::vector concatParentIds( return v; } +std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2) +{ + // Get the ID of the original AP whose transition area overlaps with the given AP, + // and set it to the parent id. + std::set ids; + for (const auto & al : lines1) { + const auto p_s = al.start_longitudinal; + const auto p_e = al.end_longitudinal; + const auto has_overlap = !(p_e < lines2.start_longitudinal || lines2.end_longitudinal < p_s); + + if (!has_overlap) { + continue; + } + + // Id the shift is overlapped, insert the shift point. Additionally, the shift which refers + // to the same object id (created by the same object) will be set. + // + // Why? : think that there are two shifts, avoiding and . + // If you register only the avoiding shift, the return-to-center shift will not be generated + // when you get too close to or over the obstacle. The return-shift can be handled with + // addReturnShift(), but it maybe reasonable to register the return-to-center shift for the + // object at the same time as registering the avoidance shift to remove the complexity of the + // addReturnShift(). + for (const auto & al_local : lines1) { + if (al_local.object.object.object_id == al.object.object.object_id) { + ids.insert(al_local.id); + } + } + } + return std::vector(ids.begin(), ids.end()); +} + double lerpShiftLengthOnArc(double arc, const AvoidLine & ap) { if (ap.start_longitudinal <= arc && arc < ap.end_longitudinal) { @@ -1010,4 +1042,81 @@ double extendToRoadShoulderDistanceWithPolygon( } return updated_to_road_shoulder_distance; } + +AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line) +{ + AvoidLineArray ret{line}; + fillAdditionalInfoFromPoint(data, ret); + return ret.front(); +} + +void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines) +{ + if (lines.empty()) { + return; + } + + const auto & path = data.reference_path; + const auto & arc = data.arclength_from_ego; + + // calc longitudinal + for (auto & sl : lines) { + sl.start_idx = findNearestIndex(path.points, sl.start.position); + sl.start_longitudinal = arc.at(sl.start_idx); + sl.end_idx = findNearestIndex(path.points, sl.end.position); + sl.end_longitudinal = arc.at(sl.end_idx); + } +} + +void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLineArray & lines) +{ + const auto & path = data.reference_path; + const auto & arc = data.arclength_from_ego; + + for (auto & sl : lines) { + sl.start_idx = findPathIndexFromArclength(arc, sl.start_longitudinal); + sl.start = path.points.at(sl.start_idx).point.pose; + sl.end_idx = findPathIndexFromArclength(arc, sl.end_longitudinal); + sl.end = path.points.at(sl.end_idx).point.pose; + } +} + +AvoidLineArray combineRawShiftLinesWithUniqueCheck( + const AvoidLineArray & base_lines, const AvoidLineArray & added_lines) +{ + // TODO(Horibe) parametrize + const auto isSimilar = [](const AvoidLine & a, const AvoidLine & b) { + using tier4_autoware_utils::calcDistance2d; + if (calcDistance2d(a.start, b.start) > 1.0) { + return false; + } + if (calcDistance2d(a.end, b.end) > 1.0) { + return false; + } + if (std::abs(a.end_shift_length - b.end_shift_length) > 0.5) { + return false; + } + return true; + }; + const auto hasSameObjectId = [](const auto & a, const auto & b) { + return a.object.object.object_id == b.object.object.object_id; + }; + + auto combined = base_lines; // initialized + for (const auto & added_line : added_lines) { + bool skip = false; + + for (const auto & base_line : base_lines) { + if (hasSameObjectId(added_line, base_line) && isSimilar(added_line, base_line)) { + skip = true; + break; + } + } + if (!skip) { + combined.push_back(added_line); + } + } + + return combined; +} } // namespace behavior_path_planner::utils::avoidance