diff --git a/.markdown-link-check.json b/.markdown-link-check.json index 331095d38dd4..dec3db1ad1c8 100644 --- a/.markdown-link-check.json +++ b/.markdown-link-check.json @@ -3,6 +3,9 @@ "ignorePatterns": [ { "pattern": "^http://localhost" + }, + { + "pattern": "^https://github.com/.*/discussions/new" } ], "retryOn429": true, 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 f1adeee1fdf2..72a153ed7d06 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 @@ -157,8 +157,7 @@ class AvoidanceModule : public SceneModuleInterface // debug mutable DebugData debug_data_; void setDebugData(const PathShifter & shifter, const DebugData & debug); - mutable std::vector debug_avoidance_initializer_for_object; - mutable std::vector debug_avoidance_initializer_for_shift_point; + mutable std::vector debug_avoidance_initializer_for_shift_point_; // ===================================== // ========= helper functions ========== // ===================================== diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index e297fcbde2d2..cb03e4b5176f 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -129,7 +129,7 @@ AvoidanceDebugMsgArray BehaviorTreeManager::getAvoidanceDebugMsgArray() const auto avoidance_module = std::find_if( scene_modules_.begin(), scene_modules_.end(), [](const std::shared_ptr & module_ptr) { - return module_ptr->current_state_ == BT::NodeStatus::SUCCESS; + return module_ptr->name() == "Avoidance"; }); if (avoidance_module != scene_modules_.end()) { const auto & ptr = avoidance_module->get()->getAvoidanceDebugMsgArray(); 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 ee0fb1557474..b03282c735ec 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 @@ -77,8 +77,6 @@ bool AvoidanceModule::isExecutionReady() const { DebugData debug; static_cast(calcAvoidancePlanningData(debug)); - debug_avoidance_msg_array_ptr_ = - std::make_shared(debug.avoidance_debug_msg_array); } if (current_state_ == BT::NodeStatus::RUNNING) { @@ -95,8 +93,6 @@ BT::NodeStatus AvoidanceModule::updateState() DebugData debug; const auto avoid_data = calcAvoidancePlanningData(debug); const bool has_avoidance_target = !avoid_data.objects.empty(); - debug_avoidance_msg_array_ptr_ = - std::make_shared(debug.avoidance_debug_msg_array); if (!is_plan_running && !has_avoidance_target) { current_state_ = BT::NodeStatus::SUCCESS; } else { @@ -298,8 +294,13 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( // debug { - debug_avoidance_initializer_for_object = avoidance_debug_msg_array; - debug.avoidance_debug_msg_array.avoidance_info = std::move(avoidance_debug_msg_array); + auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info; + debug_data_avoidance = avoidance_debug_msg_array; + debug_data_avoidance.insert( + debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point_.begin(), + debug_avoidance_initializer_for_shift_point_.end()); + debug_avoidance_msg_array_ptr_ = + std::make_shared(debug_data_.avoidance_debug_msg_array); debug.farthest_linestring_from_overhang = std::make_shared(debug_linestring); debug.current_lanelets = std::make_shared(current_lanes); @@ -595,14 +596,7 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( avoidance_debug_msg_array.push_back(avoidance_debug_msg); } - { - debug_avoidance_initializer_for_shift_point = std::move(avoidance_debug_msg_array); - auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info; - debug_data_avoidance = debug_avoidance_initializer_for_object; - debug_data_avoidance.insert( - debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point.begin(), - debug_avoidance_initializer_for_shift_point.end()); - } + debug_avoidance_initializer_for_shift_point_ = std::move(avoidance_debug_msg_array); fillAdditionalInfoFromLongitudinal(avoid_points); return avoid_points; @@ -2177,8 +2171,6 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); out.path_candidate = std::make_shared(planCandidate()); - debug_avoidance_msg_array_ptr_ = - std::make_shared(debug_data_.avoidance_debug_msg_array); return out; } @@ -2401,9 +2393,6 @@ void AvoidanceModule::updateData() { debug_data_ = DebugData(); avoidance_data_ = calcAvoidancePlanningData(debug_data_); - const auto avoidance_debug_msgs = debug_data_.avoidance_debug_msg_array.avoidance_info; - debug_avoidance_msg_array_ptr_ = - std::make_shared(debug_data_.avoidance_debug_msg_array); // TODO(Horibe): this is not tested yet, disable now. updateRegisteredObject(avoidance_data_.objects); @@ -2551,6 +2540,7 @@ void AvoidanceModule::initVariables() path_shifter_ = PathShifter{}; debug_avoidance_msg_array_ptr_.reset(); + debug_avoidance_initializer_for_shift_point_.clear(); debug_data_ = DebugData(); registered_raw_shift_points_ = {}; 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 f7ff8c9f6c1e..046b907268e0 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 @@ -235,7 +235,8 @@ class MPTOptimizer void calcVelocity( std::vector & ref_points, - const std::vector & points) const; + const std::vector & points, + const double yaw_thresh) const; void calcCurvature(std::vector & ref_points) const; @@ -299,6 +300,10 @@ 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 yaw_threshold) const; + public: MPTOptimizer( const bool is_showing_debug_info, const TrajectoryParam & traj_param, diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp index 602c5db22e0a..e9f9b1be23fb 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -27,6 +27,7 @@ #include "boost/optional/optional_fwd.hpp" #include +#include #include #include @@ -188,14 +189,19 @@ T clipBackwardPoints( template T clipBackwardPoints( - const T & points, const geometry_msgs::msg::Point pos, const double backward_length, - const double delta_length) + const T & points, const geometry_msgs::msg::Pose pose, const double backward_length, + const double delta_length, const double delta_yaw) { if (points.empty()) { return T{}; } - const size_t target_idx = tier4_autoware_utils::findNearestIndex(points, pos); + const auto target_idx_optional = tier4_autoware_utils::findNearestIndex( + points, pose, std::numeric_limits::max(), delta_yaw); + + const size_t target_idx = target_idx_optional + ? *target_idx_optional + : tier4_autoware_utils::findNearestIndex(points, pose.position); const int begin_idx = std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index c6f637416774..c8a6840f67d2 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -286,16 +286,32 @@ std::vector MPTOptimizer::getReferencePoints( // interpolate and crop backward const auto interpolated_points = interpolation_utils::getInterpolatedPoints( smoothed_points, mpt_param_.delta_arc_length_for_mpt_points); - const auto cropped_interpolated_points = points_utils::clipBackwardPoints( - interpolated_points, current_ego_pose_.position, traj_param_.backward_fixing_distance, - mpt_param_.delta_arc_length_for_mpt_points); + const auto interpolated_points_with_yaw = + points_utils::convertToPosesWithYawEstimation(interpolated_points); + const auto cropped_interpolated_points_with_yaw = points_utils::clipBackwardPoints( + interpolated_points_with_yaw, current_ego_pose_, traj_param_.backward_fixing_distance, + mpt_param_.delta_arc_length_for_mpt_points, + traj_param_.delta_yaw_threshold_for_closest_point); + const auto cropped_interpolated_points = + points_utils::convertToPoints(cropped_interpolated_points_with_yaw); return points_utils::convertToReferencePoints(cropped_interpolated_points); } // calc non fixed traj points - const size_t seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(smoothed_points, fixed_ref_points.back().p); + const auto fixed_ref_points_with_yaw = points_utils::convertToPosesWithYawEstimation( + points_utils::convertToPoints(fixed_ref_points)); + const auto seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex( + smoothed_points, fixed_ref_points_with_yaw.back(), std::numeric_limits::max(), + traj_param_.delta_yaw_threshold_for_closest_point); + + if (!seg_idx_optional) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, + "cannot find nearest segment index in getReferencePoints"); + return std::vector{}; + } + const auto seg_idx = *seg_idx_optional; const auto non_fixed_traj_points = std::vector{ smoothed_points.begin() + seg_idx, smoothed_points.end()}; @@ -322,7 +338,7 @@ std::vector MPTOptimizer::getReferencePoints( // set some information to reference points considering fix kinematics trimPoints(ref_points); calcOrientation(ref_points); - calcVelocity(ref_points, smoothed_points); + calcVelocity(ref_points, smoothed_points, traj_param_.delta_yaw_threshold_for_closest_point); calcCurvature(ref_points); calcArcLength(ref_points); calcPlanningFromEgo( @@ -385,8 +401,9 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) */ // assign fix kinematics - const size_t nearest_ref_idx = - tier4_autoware_utils::findNearestIndex(ref_points, current_ego_pose_.position); + const size_t nearest_ref_idx = findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(ref_points), current_ego_pose_, + traj_param_.delta_yaw_threshold_for_closest_point); // calculate cropped_ref_points.at(nearest_ref_idx) with yaw const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose { @@ -422,8 +439,9 @@ std::vector MPTOptimizer::getFixedReferencePoints( } const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const int nearest_prev_ref_idx = - tier4_autoware_utils::findNearestIndex(prev_ref_points, current_ego_pose_.position); + const int nearest_prev_ref_idx = static_cast(findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(prev_ref_points), current_ego_pose_, + traj_param_.delta_yaw_threshold_for_closest_point)); // calculate begin_prev_ref_idx const int begin_prev_ref_idx = [&]() { @@ -1174,6 +1192,19 @@ std::vector MPTOptimizer::get return traj_points; } +size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double yaw_threshold) const +{ + const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + + const auto nearest_idx_optional = tier4_autoware_utils::findNearestIndex( + points_with_yaw, pose, std::numeric_limits::max(), yaw_threshold); + return nearest_idx_optional + ? *nearest_idx_optional + : tier4_autoware_utils::findNearestIndex(points_with_yaw, pose.position); +} + void MPTOptimizer::calcOrientation(std::vector & ref_points) const { const auto yaw_angles = slerpYawFromReferencePoints(ref_points); @@ -1188,11 +1219,16 @@ void MPTOptimizer::calcOrientation(std::vector & ref_points) con void MPTOptimizer::calcVelocity( std::vector & ref_points, - const std::vector & points) const + const std::vector & points, + const double yaw_thresh) const { + const auto ref_points_with_yaw = + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); for (size_t i = 0; i < ref_points.size(); i++) { - ref_points.at(i).v = points[tier4_autoware_utils::findNearestIndex(points, ref_points.at(i).p)] - .longitudinal_velocity_mps; + ref_points.at(i).v = + points[findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(points), ref_points_with_yaw.at(i), yaw_thresh)] + .longitudinal_velocity_mps; } } @@ -1277,8 +1313,11 @@ void MPTOptimizer::calcExtraPoints( if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const size_t prev_idx = - tier4_autoware_utils::findNearestIndex(prev_ref_points, ref_points.at(i).p); + const auto ref_points_with_yaw = + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); + const size_t prev_idx = findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(prev_ref_points), ref_points_with_yaw.at(i), + traj_param_.delta_yaw_threshold_for_closest_point); const double dist_to_nearest_prev_ref = tier4_autoware_utils::calcDistance2d(prev_ref_points.at(prev_idx), ref_points.at(i)); if (dist_to_nearest_prev_ref < 1.0 && prev_ref_points.at(prev_idx).near_objects) { @@ -1337,7 +1376,7 @@ void MPTOptimizer::calcBounds( BoundsCandidates filtered_bounds_candidates; for (const auto & bounds_candidate : bounds_candidates) { // Step 1. Bounds is continuous to the previous one, - // and the overlapped signed length is longer than vehice width + // and the overlapped signed length is longer than vehicle width // overlapped_signed_length already considers vehicle width. const double overlapped_signed_length = calcOverlappedBoundsSignedLength(prev_continuous_bounds, bounds_candidate); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 73ab86215c5d..36c1834f9d27 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1387,16 +1387,22 @@ ObstacleAvoidancePlanner::alignVelocity( for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); - const auto & target_pos = fine_traj_points_with_vel[i].pose.position; - const size_t closest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pos); + const auto & target_pose = fine_traj_points_with_vel[i].pose; + const auto closest_seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex( + truncated_points, target_pose, std::numeric_limits::max(), + traj_param_.delta_yaw_threshold_for_closest_point); + + const auto closest_seg_idx = + closest_seg_idx_optional + ? *closest_seg_idx_optional + : tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pose.position); // lerp z fine_traj_points_with_vel[i].pose.position.z = - lerpPoseZ(truncated_points, target_pos, closest_seg_idx); + lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx); // lerp vx - const double target_vel = lerpTwistX(truncated_points, target_pos, closest_seg_idx); + const double target_vel = lerpTwistX(truncated_points, target_pose.position, closest_seg_idx); if (i >= zero_vel_fine_traj_idx) { fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0; } else if (target_vel < 1e-6) { // NOTE: velocity may be negative due to linear interpolation