From 5327ab355f9c4336e9b45232e5c8effa8e3140e4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 31 Aug 2022 17:45:29 +0900 Subject: [PATCH] fix(obstacle_avoidance_planner): deal with self-crossing and u-turn path (#1725) * fix(obstacle_avoidance_planner): deal with self-crossing and u-turn path Signed-off-by: Takayuki Murooka * fix build error Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../common_structs.hpp | 3 + .../mpt_optimizer.hpp | 4 - .../src/costmap_generator.cpp | 2 + .../src/cv_utils.cpp | 2 + .../src/eb_path_optimizer.cpp | 7 +- .../src/mpt_optimizer.cpp | 51 ++++---- .../obstacle_avoidance_planner/src/node.cpp | 109 +++++++++++------- 7 files changed, 108 insertions(+), 70 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 5805ce4753e0a..667c0916e2aba 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -203,6 +203,9 @@ struct TrajectoryParam double acceleration_for_non_deceleration_range; int num_fix_points_for_extending; double max_dist_for_extending_end_point; + + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; }; struct MPTParam diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index fe9c91ff22c65..1c12ece517e89 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -294,10 +294,6 @@ class MPTOptimizer const std::vector & ref_points, std::shared_ptr debug_data_ptr) const; - size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) const; - public: MPTOptimizer( const bool is_showing_debug_info, const TrajectoryParam & traj_param, diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp index 86322802e941c..078a453cb461a 100644 --- a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp +++ b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp @@ -85,6 +85,7 @@ bool isAvoidingObject( return false; } + // TODO(murooka) remove findNearestIndex without any constraints const int nearest_idx = motion_utils::findNearestIndex( path_points, object.kinematics.initial_pose_with_covariance.pose.position); const auto nearest_path_point = path_points[nearest_idx]; @@ -272,6 +273,7 @@ cv::Mat CostmapGenerator::drawObstaclesOnImage( // fill between objects in the same side const auto get_closest_obj_point = [&](size_t idx) { + // TODO(murooka) remove findNearestIndex without any constraints const auto & path_point = path_points.at(motion_utils::findNearestIndex(path_points, obj_positions.at(idx))); double min_dist = std::numeric_limits::min(); diff --git a/planning/obstacle_avoidance_planner/src/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/cv_utils.cpp index 3a0275f1550d8..ab77f05dd9f7b 100644 --- a/planning/obstacle_avoidance_planner/src/cv_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/cv_utils.cpp @@ -213,6 +213,7 @@ std::vector getCVPolygon( const std::vector & path_points, const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) { + // TODO(murooka) remove findNearestIndex without any constraints const int nearest_idx = motion_utils::findNearestIndex( path_points, object.kinematics.initial_pose_with_covariance.pose.position); const auto nearest_path_point = path_points[nearest_idx]; @@ -249,6 +250,7 @@ std::vector getExtendedCVPolygon( } const Edges edges = optional_edges.get(); + // TODO(murooka) remove findNearestIndex without any constraints const int nearest_polygon_idx = motion_utils::findNearestIndex(points_in_image, edges.origin); std::vector cv_polygon; if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index 70a9c1cec08e0..c50f8d724aaf4 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -236,10 +236,9 @@ std::vector EBPathOptimizer::getFixedPoints( std::vector empty_points; return empty_points; } - const auto opt_begin_idx = motion_utils::findNearestIndex( - prev_trajs->smoothed_trajectory, ego_pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - const int begin_idx = opt_begin_idx ? *opt_begin_idx : 0; + const size_t begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + prev_trajs->smoothed_trajectory, ego_pose, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold); const int backward_fixing_idx = std::max( static_cast( begin_idx - diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index f06fbe2120b90..3cb66bb7a5087 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -190,6 +190,18 @@ std::vector slerpYawFromReferencePoints(const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold, const double yaw_threshold) +{ + const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + + const auto nearest_idx_optional = + motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); + return nearest_idx_optional ? *nearest_idx_optional + : motion_utils::findNearestIndex(points_with_yaw, pose.position); +} } // namespace MPTOptimizer::MPTOptimizer( @@ -434,10 +446,10 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) */ // assign fix kinematics - const size_t nearest_ref_idx = findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(ref_points), current_ego_pose_, - traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); + const size_t nearest_ref_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)), + current_ego_pose_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold); // calculate cropped_ref_points.at(nearest_ref_idx) with yaw const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose { @@ -473,10 +485,11 @@ std::vector MPTOptimizer::getFixedReferencePoints( } const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const int nearest_prev_ref_idx = static_cast(findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(prev_ref_points), current_ego_pose_, - traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point)); + const int nearest_prev_ref_idx = + static_cast(motion_utils::findFirstNearestIndexWithSoftConstraints( + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(prev_ref_points)), + current_ego_pose_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold)); // calculate begin_prev_ref_idx const int begin_prev_ref_idx = [&]() { @@ -723,8 +736,14 @@ boost::optional MPTOptimizer::executeOptimization( const size_t D_x = vehicle_model_ptr_->getDimX(); if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) { - const size_t seg_idx = - motion_utils::findNearestSegmentIndex(prev_trajs->mpt_ref_points, ref_points.front().p); + geometry_msgs::msg::Pose ref_front_point; + ref_front_point.position = ref_points.front().p; + ref_front_point.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_points.front().yaw); + + const size_t seg_idx = findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(prev_trajs->mpt_ref_points), ref_front_point, + traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold); double offset = motion_utils::calcLongitudinalOffsetToSegment( prev_trajs->mpt_ref_points, seg_idx, ref_points.front().p); @@ -1223,18 +1242,6 @@ std::vector MPTOptimizer::get return traj_points; } -size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) const -{ - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); - - const auto nearest_idx_optional = - motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points_with_yaw, pose.position); -} - void MPTOptimizer::calcOrientation(std::vector & ref_points) const { const auto yaw_angles = slerpYawFromReferencePoints(ref_points); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 4fe6aca6d4992..3802dde0e067a 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -38,38 +38,27 @@ namespace { -template -size_t searchExtendedZeroVelocityIndex( - const std::vector & fine_points, const std::vector & vel_points) -{ - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points); - const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; - return motion_utils::findNearestIndex(fine_points, vel_points.at(zero_vel_idx).pose.position); -} - bool isPathShapeChanged( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, const std::unique_ptr> & prev_path_points, - const double max_mpt_length, const double max_path_shape_change_dist, - const double delta_yaw_threshold) + const double max_mpt_length, const double max_path_shape_change_dist, const double dist_threshold, + const double yaw_threshold) { if (!prev_path_points) { return false; } // truncate prev points from ego pose to fixed end points - const auto opt_prev_begin_idx = motion_utils::findNearestIndex( - *prev_path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); - const size_t prev_begin_idx = opt_prev_begin_idx ? *opt_prev_begin_idx : 0; + const auto prev_begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + *prev_path_points, ego_pose, dist_threshold, yaw_threshold); const auto truncated_prev_points = points_utils::clipForwardPoints(*prev_path_points, prev_begin_idx, max_mpt_length); // truncate points from ego pose to fixed end points - const auto opt_begin_idx = motion_utils::findNearestIndex( - path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); - const size_t begin_idx = opt_begin_idx ? *opt_begin_idx : 0; + const auto begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_points, ego_pose, dist_threshold, yaw_threshold); const auto truncated_points = points_utils::clipForwardPoints(path_points, begin_idx, max_mpt_length); @@ -200,16 +189,60 @@ std::tuple, std::vector> calcVehicleCirclesInfo( } } +template +[[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ + const auto nearest_idx_optional = + motion_utils::findNearestIndex(points, pose, dist_threshold, yaw_threshold); + return nearest_idx_optional ? *nearest_idx_optional + : motion_utils::findNearestIndex(points, pose.position); +} + +template <> [[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + return findNearestIndexWithSoftYawConstraints( + points_with_yaw, pose, dist_threshold, yaw_threshold); +} + +template +[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ const auto nearest_idx_optional = - motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); + motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points_with_yaw, pose.position); + : motion_utils::findNearestSegmentIndex(points, pose.position); +} + +template <> +[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold, const double yaw_threshold) +{ + const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + + return findNearestSegmentIndexWithSoftYawConstraints( + points_with_yaw, pose, dist_threshold, yaw_threshold); +} + +template +size_t searchExtendedZeroVelocityIndex( + const std::vector & fine_points, const std::vector & vel_points, + const double yaw_threshold) +{ + const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points); + const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; + return findNearestIndexWithSoftYawConstraints( + fine_points, vel_points.at(zero_vel_idx).pose, std::numeric_limits::max(), + yaw_threshold); } } // namespace @@ -344,6 +377,11 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n declare_parameter("object.avoiding_object_type.pedestrian", true); traj_param_.is_avoiding_animal = declare_parameter("object.avoiding_object_type.animal", true); + + // ego nearest search + traj_param_.ego_nearest_dist_threshold = + declare_parameter("ego_nearest_dist_threshold"); + traj_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); } { // elastic band parameter @@ -986,8 +1024,8 @@ bool ObstacleAvoidancePlanner::checkReplan( traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; if (isPathShapeChanged( current_ego_pose_, path_points, prev_path_points_ptr_, max_mpt_length, - max_path_shape_change_dist_for_replan_, - traj_param_.delta_yaw_threshold_for_closest_point)) { + max_path_shape_change_dist_for_replan_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold)) { RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path shape was changed."); resetPrevOptimization(); return true; @@ -1110,15 +1148,9 @@ void ObstacleAvoidancePlanner::calcVelocity( std::vector & traj_points) const { for (size_t i = 0; i < traj_points.size(); i++) { - const size_t nearest_seg_idx = [&]() { - const auto opt_seg_idx = motion_utils::findNearestSegmentIndex( - path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - if (opt_seg_idx) { - return opt_seg_idx.get(); - } - return motion_utils::findNearestSegmentIndex(path_points, traj_points.at(i).pose.position); - }(); + const size_t nearest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( + path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, + traj_param_.delta_yaw_threshold_for_closest_point); // add this line not to exceed max index size const size_t max_idx = std::min(nearest_seg_idx + 1, path_points.size() - 1); @@ -1144,8 +1176,9 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( const auto & map_info = cv_maps.map_info; const auto & road_clearance_map = cv_maps.clearance_map; - const size_t nearest_idx = - motion_utils::findNearestIndex(traj_points, current_ego_pose_.position); + const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_points, current_ego_pose_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold); // NOTE: Some end trajectory points will be ignored to check if outside the drivable area // since these points might be outside drivable area if only end reference points have high @@ -1469,8 +1502,9 @@ ObstacleAvoidancePlanner::alignVelocity( // search zero velocity index of fine_traj_points const size_t zero_vel_fine_traj_idx = [&]() { // zero velocity for being outside drivable area - const size_t zero_vel_traj_idx = - searchExtendedZeroVelocityIndex(fine_traj_points_with_path_zero_vel, traj_points); + const size_t zero_vel_traj_idx = searchExtendedZeroVelocityIndex( + fine_traj_points_with_path_zero_vel, traj_points, + traj_param_.delta_yaw_threshold_for_closest_point); // zero velocity in path points if (opt_zero_vel_path_idx) { @@ -1492,15 +1526,10 @@ ObstacleAvoidancePlanner::alignVelocity( } const auto & target_pose = fine_traj_points_with_vel[i].pose; - const auto closest_seg_idx_optional = motion_utils::findNearestSegmentIndex( + const size_t closest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( truncated_points, target_pose, traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); - const auto closest_seg_idx = - closest_seg_idx_optional - ? *closest_seg_idx_optional - : motion_utils::findNearestSegmentIndex(truncated_points, target_pose.position); - // lerp z fine_traj_points_with_vel[i].pose.position.z = lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx);