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 56a7c5e9ec2a7..f6e543932bce4 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 @@ -394,15 +394,6 @@ class AvoidanceModule : public SceneModuleInterface path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); } - // misc functions - - double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - - // TODO(Horibe): think later. - // for unique ID - mutable uint64_t original_unique_id = 0; // TODO(Horibe) remove mutable - uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; } - struct RegisteredShiftLine { UUID uuid; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index b5c6d22c124f6..381c2ef23fc9d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -414,10 +414,10 @@ struct AvoidLine : public ShiftLine double end_longitudinal = 0.0; // for unique_id - uint64_t id = 0; + UUID id{}; // for the case the point is created by merge other points - std::vector parent_ids{}; + std::vector parent_ids{}; // corresponding object ObjectData object{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp index 29c13c6cbd196..19e7e05a8429c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp @@ -22,12 +22,7 @@ #include -// #include #include -// #include -// #include -// #include -// #include namespace behavior_path_planner::utils::avoidance { @@ -47,8 +42,6 @@ class ShiftLineGenerator void setData(const std::shared_ptr & data) { data_ = data; } - void setAvoidData(const AvoidancePlanningData & avoid_data) { avoid_data_ = avoid_data; } - void setHelper(const std::shared_ptr & helper) { helper_ = helper; } void setPathShifter(const PathShifter & path_shifter) { path_shifter_ = path_shifter; } @@ -57,8 +50,6 @@ class ShiftLineGenerator void update(AvoidancePlanningData & data, DebugData & debug); - void generate(DebugData & debug); - void reset() { outline_.clear(); @@ -66,12 +57,12 @@ class ShiftLineGenerator raw_registered_.clear(); } + AvoidLineArray generate(DebugData & debug) const; + AvoidLineArray getRawShiftLine() const { return raw_; } AvoidLineArray getRawRegisteredShiftLine() const { return raw_registered_; } - AvoidLineArray getCandidateShiftLine() const { return candidate_; } - private: /** * @brief Calculate the shift points (start/end point, shift length) from the object lateral @@ -220,15 +211,8 @@ class ShiftLineGenerator */ void updateRegisteredRawShiftLines(const AvoidancePlanningData & data); - // misc functions - double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - // TODO(Horibe): think later. - // for unique ID - mutable uint64_t original_unique_id = 0; // TODO(Horibe) remove mutable - uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; } - std::shared_ptr data_; std::shared_ptr parameters_; @@ -244,10 +228,6 @@ class ShiftLineGenerator AvoidLineArray raw_; AvoidLineArray raw_registered_; - - // AvoidLineArray raw_current_; - - AvoidLineArray candidate_; }; } // namespace behavior_path_planner::utils::avoidance 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 2e04935e37336..e0eceac441f58 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 @@ -59,10 +59,9 @@ ShiftedPath toShiftedPath(const PathWithLaneId & path); ShiftLineArray toShiftLineArray(const AvoidLineArray & avoid_points); -std::vector concatParentIds( - const std::vector & ids1, const std::vector & ids2); +std::vector concatParentIds(const std::vector & ids1, const std::vector & ids2); -std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2); +std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2); double lerpShiftLengthOnArc(double arc, const AvoidLine & al); diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index ce9cedc816a19..ea9946783b35f 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include #include #include @@ -509,14 +510,16 @@ std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr) } std::string toStrInfo(const behavior_path_planner::AvoidLine & ap) { + using tier4_autoware_utils::toHexString; + std::stringstream pids; for (const auto pid : ap.parent_ids) { - pids << pid << ", "; + pids << toHexString(pid) << ", "; } const auto & ps = ap.start.position; const auto & pe = ap.end.position; std::stringstream ss; - ss << "id = " << ap.id << ", shift length: " << ap.end_shift_length + ss << "id = " << toHexString(ap.id) << ", shift length: " << ap.end_shift_length << ", start_idx: " << ap.start_idx << ", end_idx: " << ap.end_idx << ", start_dist = " << ap.start_longitudinal << ", end_dist = " << ap.end_longitudinal << ", start_shift_length: " << ap.start_shift_length << ", start: (" << ps.x << ", " << ps.y 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 56904fc044653..101547b89a3b3 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 @@ -440,31 +440,14 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de { auto path_shifter = path_shifter_; - // generator_.generate(data, debug); - - // data.raw_shift_line = generator_.getRawShiftLine(); - - const auto processed_shift_lines = generator_.getCandidateShiftLine(); - - // /** - // * STEP1: Generate avoid outlines. - // * Basically, avoid outlines are generated per target objects. - // */ - // const auto outlines = generateAvoidOutline(data, debug); - - // /** - // * STEP2: Create rough shift lines. - // */ - // data.raw_shift_line = applyPreProcess(outlines, debug); - - // /** - // * STEP3: Create candidate shift lines. - // * Merge rough shift lines, and extract new shift lines. - // */ - // const auto processed_shift_lines = generateCandidateShiftLine(data.raw_shift_line, debug); + /** + * STEP1: Create candidate shift lines. + * Merge rough shift lines, and extract new shift lines. + */ + const auto processed_shift_lines = generator_.generate(debug); /** - * Step4: Validate new shift lines. + * Step2: Validate new shift lines. * Output new shift lines only when the avoidance path which is generated from them doesn't have * huge offset from ego. */ @@ -475,7 +458,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de data.found_avoidance_path = found_new_sl || registered; /** - * STEP5: Set new shift lines. + * STEP3: Set new shift lines. * If there are new shift points, these shift points are registered in path_shifter in order to * generate candidate avoidance path. */ @@ -484,7 +467,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de } /** - * STEP6: Generate avoidance path. + * STEP4: Generate avoidance path. */ ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = @@ -494,7 +477,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de : utils::avoidance::toShiftedPath(data.reference_path); /** - * STEP7: Check avoidance path safety. + * STEP5: Check avoidance path safety. * For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ @@ -1029,13 +1012,8 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) addNewShiftLines(path_shifter_, shift_lines); - // generator_.setRawShiftLine(avoid_data_.raw_shift_line); generator_.setRawRegisteredShiftLine(shift_lines); - // current_raw_shift_lines_ = avoid_data_.raw_shift_line; - - // registerRawShiftLines(shift_lines); - const auto sl = helper_.getMainShiftLine(shift_lines); const auto sl_front = shift_lines.front(); const auto sl_back = shift_lines.back(); @@ -1205,15 +1183,11 @@ void AvoidanceModule::updateData() return; } - // // update registered shift point for new reference path & remove past objects - // updateRegisteredRawShiftLines(); - + // update shift line generator. generator_.setData(planner_data_); generator_.setPathShifter(path_shifter_); generator_.setHelper(std::make_shared(helper_)); generator_.update(avoid_data_, debug_data_); - generator_.setAvoidData(avoid_data_); - generator_.generate(debug_data_); // update shift line and check path safety. fillShiftLine(avoid_data_, debug_data_); @@ -1251,7 +1225,6 @@ void AvoidanceModule::initVariables() debug_marker_.markers.clear(); resetPathCandidate(); resetPathReference(); - original_unique_id = 0; is_avoidance_maneuver_starts = false; arrived_path_end_ = false; } diff --git a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp b/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp index b7f66f5cf679b..3b2f21cd964e8 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp @@ -17,27 +17,15 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include +#include -// #include -// #include -// #include -// #include -// #include -// #include +#include namespace behavior_path_planner::utils::avoidance { -// using motion_utils::calcLongitudinalOffsetPose; -// using motion_utils::calcSignedArcLength; -// using motion_utils::findNearestIndex; -// using tier4_autoware_utils::appendMarkerArray; -// using tier4_autoware_utils::calcDistance2d; -// using tier4_autoware_utils::calcLateralDeviation; +using tier4_autoware_utils::generateUUID; using tier4_autoware_utils::getPoint; -// using tier4_autoware_utils::getPose; -// using tier4_autoware_utils::toHexString; using tier4_planning_msgs::msg::AvoidanceDebugFactor; namespace @@ -47,7 +35,7 @@ bool isBestEffort(const std::string & policy) return policy == "best_effort"; } -AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) +AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const UUID id) { AvoidLine ret{}; @@ -65,7 +53,7 @@ AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const uint64_t return ret; } -AvoidLine fill(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) +AvoidLine fill(const AvoidLine & line1, const AvoidLine & line2, const UUID id) { AvoidLine ret{}; @@ -101,30 +89,30 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) void ShiftLineGenerator::update(AvoidancePlanningData & data, DebugData & debug) { /** - * STEP1: Generate avoid outlines. - * Basically, avoid outlines are generated per target objects. + * STEP1: Update registered shift line. */ updateRegisteredRawShiftLines(data); /** - * STEP1: Generate avoid outlines. + * STEP2: Generate avoid outlines. * Basically, avoid outlines are generated per target objects. */ outline_ = generateAvoidOutline(data, debug); -} -void ShiftLineGenerator::generate(DebugData & debug) -{ /** - * STEP2: Create rough shift lines. + * STEP3: Create rough shift lines. */ raw_ = applyPreProcess(outline_, debug); /** - * STEP3: Create candidate shift lines. - * Merge rough shift lines, and extract new shift lines. + * STEP4: Update avoidance data. */ - candidate_ = generateCandidateShiftLine(raw_, debug); + avoid_data_ = data; +} + +AvoidLineArray ShiftLineGenerator::generate(DebugData & debug) const +{ + return generateCandidateShiftLine(raw_, debug); } AvoidOutlines ShiftLineGenerator::generateAvoidOutline( @@ -231,20 +219,11 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( std::abs(feasible_shift_length - object.overhang_dist) < 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; if (infeasible) { - // RCLCPP_WARN_THROTTLE( - // rclcpp::get_logger("generator"), rclcpp::Clock{RCL_ROS_TIME}, 1000, "feasible shift - // length is not enough to avoid. "); + RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; return std::nullopt; } - // { - // RCLCPP_WARN_THROTTLE( - // rclcpp::get_logger("generator"), rclcpp::Clock{RCL_ROS_TIME}, 1000, "use feasible shift - // length. [original: (%.2f) actual: (%.2f)]", std::abs(avoiding_shift), - // feasible_relative_shift_length); - // } - return std::make_pair(feasible_shift_length, avoidance_distance); }; @@ -330,7 +309,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_avoid.end_longitudinal = to_shift_end; // misc - al_avoid.id = getOriginalShiftLineUniqueId(); + al_avoid.id = generateUUID(); al_avoid.object = o; al_avoid.object_on_right = utils::avoidance::isOnRight(o); } @@ -356,7 +335,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_return.end_shift_length = 0.0; // misc - al_return.id = getOriginalShiftLineUniqueId(); + al_return.id = generateUUID(); al_return.object = o; al_return.object_on_right = utils::avoidance::isOnRight(o); } @@ -610,7 +589,7 @@ AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine(ShiftLineData & shi found_first_start = true; } else { setEndData(al, shift, p, i, arcs.at(i)); - al.id = getOriginalShiftLineUniqueId(); + al.id = generateUUID(); merged_avoid_lines.push_back(al); setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. } @@ -620,7 +599,7 @@ AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine(ShiftLineData & shi const auto & p = path.points.at(N - 1).point.pose; const auto shift = sl.shift_line.at(N - 1); setEndData(al, shift, p, N - 1, arcs.at(N - 1)); - al.id = getOriginalShiftLineUniqueId(); + al.id = generateUUID(); merged_avoid_lines.push_back(al); } @@ -662,7 +641,7 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( continue; } - const auto merged_shift_line = merge(return_line, avoid_line, getOriginalShiftLineUniqueId()); + const auto merged_shift_line = merge(return_line, avoid_line, generateUUID()); if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { ret.push_back(outlines.at(i)); @@ -704,8 +683,7 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( for (auto & outline : ret) { if (outline.middle_lines.empty()) { - const auto new_line = - fill(outline.avoid_line, outline.return_line, getOriginalShiftLineUniqueId()); + const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -713,8 +691,7 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); if (outline.avoid_line.end_longitudinal < outline.middle_lines.front().start_longitudinal) { - const auto new_line = - fill(outline.avoid_line, outline.middle_lines.front(), getOriginalShiftLineUniqueId()); + const auto new_line = fill(outline.avoid_line, outline.middle_lines.front(), generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -722,8 +699,7 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { - const auto new_line = - fill(outline.middle_lines.back(), outline.return_line, getOriginalShiftLineUniqueId()); + const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -759,7 +735,7 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( ego_line, helper_->getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0); - const auto new_line = fill(ego_line, sorted.front(), getOriginalShiftLineUniqueId()); + const auto new_line = fill(ego_line, sorted.front(), generateUUID()); ret.push_back(new_line); debug.step1_front_shift_line.push_back(new_line); } @@ -772,7 +748,7 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( continue; } - const auto new_line = fill(sorted.at(i), sorted.at(i + 1), getOriginalShiftLineUniqueId()); + const auto new_line = fill(sorted.at(i), sorted.at(i + 1), generateUUID()); ret.push_back(new_line); debug.step1_front_shift_line.push_back(new_line); } @@ -1100,8 +1076,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( // check if there is enough distance for return. if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) - // RCLCPP_WARN_THROTTLE(rclcpp::get_logger("generator"), rclcpp::Clock{RCL_ROS_TIME}, 1000, "No - // enough distance for return."); + RCLCPP_DEBUG(rclcpp::get_logger(""), "No enough distance for return."); return ret; } @@ -1136,7 +1111,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( // shift point for prepare distance: from last shift to return-start point. if (nominal_prepare_distance > last_sl_distance) { AvoidLine al; - al.id = getOriginalShiftLineUniqueId(); + al.id = generateUUID(); al.start_idx = last_sl.end_idx; al.start = last_sl.end; al.start_longitudinal = arclength_from_ego.at(al.start_idx); @@ -1153,7 +1128,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( // shift point for return to center line { AvoidLine al; - al.id = getOriginalShiftLineUniqueId(); + al.id = generateUUID(); al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.start = avoid_data_.reference_path.points.at(al.start_idx).point.pose; @@ -1296,15 +1271,12 @@ void ShiftLineGenerator::updateRegisteredRawShiftLines(const AvoidancePlanningDa } raw_registered_ = avoid_lines; - - // registered_raw_shift_lines_ = avoid_lines; - // debug_data_.step1_registered_shift_line = registered_raw_shift_lines_; } void ShiftLineGenerator::setRawRegisteredShiftLine(const AvoidLineArray & shift_lines) { if (shift_lines.empty()) { - // RCLCPP_ERROR(getLogger(), "future is empty! return."); + RCLCPP_ERROR(rclcpp::get_logger(""), "future is empty! return."); return; } @@ -1338,7 +1310,7 @@ void ShiftLineGenerator::setRawRegisteredShiftLine(const AvoidLineArray & shift_ for (const auto & s : future_with_info) { if (s.parent_ids.empty()) { - // RCLCPP_ERROR(getLogger(), "avoid line for path_shifter must have parent_id."); + RCLCPP_ERROR(rclcpp::get_logger(""), "avoid line for path_shifter must have parent_id."); } for (const auto id : s.parent_ids) { diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index ce4c9ad7ac72d..ee558b5808d13 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -444,22 +444,24 @@ size_t findPathIndexFromArclength( return path_arclength_arr.size() - 1; } -std::vector concatParentIds( - const std::vector & ids1, const std::vector & ids2) +std::vector concatParentIds(const std::vector & ids1, const std::vector & ids2) { - std::set id_set{ids1.begin(), ids1.end()}; - for (const auto id : ids2) { - id_set.insert(id); + std::vector ret; + for (const auto id2 : ids2) { + if (std::any_of(ids1.begin(), ids1.end(), [&id2](const auto & id1) { return id1 == id2; })) { + continue; + } + ret.push_back(id2); } - const auto v = std::vector{id_set.begin(), id_set.end()}; - return v; + + return ret; } -std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2) +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; + std::vector ret; for (const auto & al : lines1) { const auto p_s = al.start_longitudinal; const auto p_e = al.end_longitudinal; @@ -469,9 +471,9 @@ std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine continue; } - ids.insert(al.id); + ret.push_back(al.id); } - return std::vector(ids.begin(), ids.end()); + return ret; } double lerpShiftLengthOnArc(double arc, const AvoidLine & ap)