From 8bdfe7cc54bca944afd1e77614730573210b8a12 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 22 Dec 2022 12:13:57 +0900 Subject: [PATCH 01/11] refactor(avoidance): generate shift line before the plan() function (#2533) * refactor(avoidance): generate shift line in updateData() Signed-off-by: satoshi-ota * refactor(avoidance): define helper function in header file Signed-off-by: satoshi-ota * fix(avoidance): fix function name Signed-off-by: satoshi-ota * fix(avoidance): fix function name Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 124 +++++++-- .../avoidance/avoidance_module_data.hpp | 11 + .../avoidance/avoidance_module.cpp | 257 ++++++++---------- 3 files changed, 215 insertions(+), 177 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 a4783a445b177..68c3efd1b75a1 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 @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -35,7 +36,12 @@ namespace behavior_path_planner { + +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; + using tier4_planning_msgs::msg::AvoidanceDebugMsg; + class AvoidanceModule : public SceneModuleInterface { public: @@ -134,10 +140,10 @@ class AvoidanceModule : public SceneModuleInterface const Point ego_position = planner_data_->self_pose->pose.position; for (const auto & left_shift : left_shift_array_) { - const double start_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, left_shift.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, left_shift.finish_pose.position); + const double start_distance = + calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); + const double finish_distance = + calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); rtc_interface_left_.updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -148,10 +154,10 @@ class AvoidanceModule : public SceneModuleInterface } for (const auto & right_shift : right_shift_array_) { - const double start_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, right_shift.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, right_shift.finish_pose.position); + const double start_distance = + calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); + const double finish_distance = + calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); rtc_interface_right_.updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -200,6 +206,7 @@ class AvoidanceModule : public SceneModuleInterface void fillObjectMovingTime(ObjectData & object_data) const; void compensateDetectionLost( ObjectDataArray & target_objects, ObjectDataArray & other_objects) const; + void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; // data used in previous planning ShiftedPath prev_output_; @@ -225,14 +232,13 @@ class AvoidanceModule : public SceneModuleInterface void updateRegisteredObject(const ObjectDataArray & objects); // -- for shift point generation -- - AvoidLineArray calcShiftLines(AvoidLineArray & current_raw_shift_lines, DebugData & debug) const; + AvoidLineArray applyPreProcessToRawShiftLines( + AvoidLineArray & current_raw_shift_points, DebugData & debug) const; // shift point generation: generator double getShiftLength( const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const; AvoidLineArray calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const; - double getRightShiftBound() const; - double getLeftShiftBound() const; // shift point generation: combiner AvoidLineArray combineRawShiftLinesWithUniqueCheck( @@ -268,7 +274,7 @@ class AvoidanceModule : public SceneModuleInterface void fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_lines) const; // -- for new shift point approval -- - boost::optional findNewShiftLine( + AvoidLineArray findNewShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const; void addShiftLineIfApproved(const AvoidLineArray & point); void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const; @@ -300,33 +306,91 @@ class AvoidanceModule : public SceneModuleInterface void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; - // ===================================== + // ========= helper functions ========== - // ===================================== + + double getEgoSpeed() const + { + return std::abs(planner_data_->self_odometry->twist.twist.linear.x); + } + + double getNominalAvoidanceEgoSpeed() const + { + return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); + } + + double getSharpAvoidanceEgoSpeed() const + { + return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); + } + + double getNominalPrepareDistance() const + { + const auto & p = parameters_; + const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. + const auto nominal_distance = + std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); + return nominal_distance + epsilon_m; + } + + double getNominalAvoidanceDistance(const double shift_length) const + { + const auto & p = parameters_; + const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( + shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); + + return std::max(p->min_avoidance_distance, distance_by_jerk); + } + + double getSharpAvoidanceDistance(const double shift_length) const + { + const auto & p = parameters_; + const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( + shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); + + return std::max(p->min_avoidance_distance, distance_by_jerk); + } + + double getRightShiftBound() const + { + // TODO(Horibe) write me. Real lane boundary must be considered here. + return -parameters_->max_right_shift_length; + } + + double getLeftShiftBound() const + { + // TODO(Horibe) write me. Real lane boundary must be considered here. + return parameters_->max_left_shift_length; + } + + double getCurrentShift() const + { + return prev_output_.shift_length.at( + findNearestIndex(prev_output_.path.points, getEgoPosition())); + } + + double getCurrentLinearShift() const + { + return prev_linear_shift_path_.shift_length.at( + findNearestIndex(prev_linear_shift_path_.path.points, getEgoPosition())); + } + + double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } + + Point getEgoPosition() const { return planner_data_->self_pose->pose.position; } + + Pose getEgoPose() const { return planner_data_->self_pose->pose; } + + Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const; PathWithLaneId calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const; + const std::shared_ptr & planner_data, const Pose & pose) const; // 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++; } - double getNominalAvoidanceDistance(const double shift_length) const; - double getNominalPrepareDistance() const; - double getNominalAvoidanceEgoSpeed() const; - - double getSharpAvoidanceDistance(const double shift_length) const; - double getSharpAvoidanceEgoSpeed() const; - - double getEgoSpeed() const; - Point getEgoPosition() const; - PoseStamped getEgoPose() const; - PoseStamped getUnshiftedEgoPose(const ShiftedPath & prev_path) const; - double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - double getCurrentShift() const; - double getCurrentLinearShift() const; - /** * avoidance module misc data */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 25b29352973c1..426387c939165 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -286,11 +286,22 @@ struct AvoidancePlanningData // current driving lanelet lanelet::ConstLanelets current_lanelets; + // output path + ShiftedPath candidate_path; + // avoidance target objects ObjectDataArray target_objects; // the others ObjectDataArray other_objects; + + // raw shift point + AvoidLineArray unapproved_raw_sl{}; + + // new shift point + AvoidLineArray unapproved_new_sl{}; + + bool avoiding_now{false}; }; /* 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 1f7979c9b726c..61963bb9f0aaa 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 @@ -50,6 +50,22 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcLateralDeviation; using tier4_planning_msgs::msg::AvoidanceDebugFactor; +namespace +{ + +AvoidLine getNonStraightShiftLine(const AvoidLineArray & shift_lines) +{ + for (const auto & sl : shift_lines) { + if (fabs(sl.getRelativeLength()) > 0.01) { + return sl; + } + } + + return {}; +} + +} // namespace + AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) : SceneModuleInterface{name, node}, @@ -146,7 +162,7 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // reference pose const auto reference_pose = getUnshiftedEgoPose(prev_output_); - data.reference_pose = reference_pose.pose; + data.reference_pose = reference_pose; // center line path (output of this function must have size > 1) const auto center_path = calcCenterLinePath(planner_data_, reference_pose); @@ -177,8 +193,8 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // lanelet info data.current_lanelets = util::calcLaneAroundPose( - planner_data_->route_handler, reference_pose.pose, - planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length); + planner_data_->route_handler, reference_pose, planner_data_->parameters.forward_path_length, + planner_data_->parameters.backward_path_length); // target objects for avoidance fillAvoidanceTargetObjects(data, debug); @@ -503,6 +519,51 @@ void AvoidanceModule::fillObjectMovingTime(ObjectData & object_data) const } } +void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const +{ + constexpr double AVOIDING_SHIFT_THR = 0.1; + data.avoiding_now = std::abs(getCurrentShift()) > AVOIDING_SHIFT_THR; + + auto path_shifter = path_shifter_; + + /** + * STEP 1 + * Create raw shift points from target object. The lateral margin between the ego and the target + * object varies depending on the relative speed between the ego and the target object. + */ + data.unapproved_raw_sl = calcRawShiftLinesFromObjects(data.target_objects); + + /** + * STEP 2 + * Modify the raw shift points. (Merging, Trimming) + */ + const auto processed_raw_sp = applyPreProcessToRawShiftLines(data.unapproved_raw_sl, debug); + + /** + * STEP 3 + * Find new shift point + */ + data.unapproved_new_sl = findNewShiftLine(processed_raw_sp, path_shifter); + + /** + * STEP 4 + * If there are new shift points, these shift points are registered in path_shifter. + */ + if (!data.unapproved_new_sl.empty()) { + addNewShiftLines(path_shifter, data.unapproved_new_sl); + } + + /** + * STEP 5 + * Generate avoidance path. + */ + data.candidate_path = generateAvoidancePath(path_shifter); + + { + debug.current_raw_shift = data.unapproved_raw_sl; + } +} + /** * updateRegisteredRawShiftLines * @@ -535,16 +596,9 @@ void AvoidanceModule::updateRegisteredRawShiftLines() debug_data_.registered_raw_shift = registered_raw_shift_lines_; } -AvoidLineArray AvoidanceModule::calcShiftLines( +AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_lines, DebugData & debug) const { - /** - * Generate raw_shift_lines (shift length, avoidance start point, end point, return point, etc) - * for each object. These raw shift points are merged below to compute appropriate shift points. - */ - current_raw_shift_lines = calcRawShiftLinesFromObjects(avoidance_data_.target_objects); - debug.current_raw_shift = current_raw_shift_lines; - /** * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. @@ -1744,7 +1798,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end = getEgoPose().pose; + last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; last_sl.end_shift_length = getCurrentBaseShift(); } @@ -1780,7 +1834,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end = getEgoPose().pose; + last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; last_sl.end_shift_length = current_base_shift; } @@ -1885,18 +1939,6 @@ void AvoidanceModule::addReturnShiftLineFromEgo( DEBUG_PRINT("Return Shift is added."); } -double AvoidanceModule::getRightShiftBound() const -{ - // TODO(Horibe) write me. Real lane boundary must be considered here. - return -parameters_->max_right_shift_length; -} - -double AvoidanceModule::getLeftShiftBound() const -{ - // TODO(Horibe) write me. Real lane boundary must be considered here. - return parameters_->max_left_shift_length; -} - // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep // extending during avoidance module // TODO(murooka) freespace during turning in intersection where there is no neighbour lanes @@ -2090,7 +2132,7 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted // TODO(Horibe) clean up functions: there is a similar code in util as well. PathWithLaneId AvoidanceModule::calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const + const std::shared_ptr & planner_data, const Pose & pose) const { const auto & p = planner_data->parameters; const auto & route_handler = planner_data->route_handler; @@ -2121,9 +2163,9 @@ PathWithLaneId AvoidanceModule::calcCenterLinePath( p.backward_path_length, longest_dist_to_shift_line, backward_length); const lanelet::ConstLanelets current_lanes = - util::calcLaneAroundPose(route_handler, pose.pose, p.forward_path_length, backward_length); + util::calcLaneAroundPose(route_handler, pose, p.forward_path_length, backward_length); centerline_path = util::getCenterLinePath( - *route_handler, current_lanes, pose.pose, backward_length, p.forward_path_length, p); + *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); // for debug: check if the path backward distance is same as the desired length. // { @@ -2217,14 +2259,10 @@ boost::optional AvoidanceModule::calcIntersectionShiftLine( BehaviorModuleOutput AvoidanceModule::plan() { - DEBUG_PRINT("AVOIDANCE plan"); + const auto & data = avoidance_data_; resetPathCandidate(); - const auto shift_lines = calcShiftLines(current_raw_shift_lines_, debug_data_); - - const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter_); - /** * Has new shift point? * Yes -> Is it approved? @@ -2234,26 +2272,20 @@ BehaviorModuleOutput AvoidanceModule::plan() * Yes -> clear WAIT_APPROVAL state. * No -> do nothing. */ - if (new_shift_lines) { - debug_data_.new_shift_lines = *new_shift_lines; - DEBUG_PRINT("new_shift_lines size = %lu", new_shift_lines->size()); - printShiftLines(*new_shift_lines, "new_shift_lines"); - int i = new_shift_lines->size() - 1; - for (; i > 0; i--) { - if (fabs(new_shift_lines->at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } - if (new_shift_lines->at(i).getRelativeLength() > 0.0) { + if (!data.unapproved_new_sl.empty()) { + debug_data_.new_shift_lines = data.unapproved_new_sl; + DEBUG_PRINT("new_shift_lines size = %lu", data.unapproved_new_sl.size()); + printShiftLines(data.unapproved_new_sl, "new_shift_lines"); + + const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); + if (sl.getRelativeLength() > 0.0) { removePreviousRTCStatusRight(); - } else if (new_shift_lines->at(i).getRelativeLength() < 0.0) { + } else if (sl.getRelativeLength() < 0.0) { removePreviousRTCStatusLeft(); } else { RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); } - addShiftLineIfApproved(*new_shift_lines); + addShiftLineIfApproved(data.unapproved_new_sl); } else if (isWaitingApproval()) { clearWaitingApproval(); removeCandidateRTCStatus(); @@ -2304,35 +2336,22 @@ BehaviorModuleOutput AvoidanceModule::plan() CandidateOutput AvoidanceModule::planCandidate() const { - DEBUG_PRINT("AVOIDANCE planCandidate start"); + const auto & data = avoidance_data_; + CandidateOutput output; - auto path_shifter = path_shifter_; - auto debug_data = debug_data_; - auto current_raw_shift_lines = current_raw_shift_lines_; + auto shifted_path = data.candidate_path; - const auto shift_lines = calcShiftLines(current_raw_shift_lines, debug_data); - const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter); - if (new_shift_lines) { - addNewShiftLines(path_shifter, *new_shift_lines); - } + if (!data.unapproved_new_sl.empty()) { // clip from shift start index for visualize + clipByMinStartIdx(data.unapproved_new_sl, shifted_path.path); - auto shifted_path = generateAvoidancePath(path_shifter); + const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); + const auto sl_front = data.unapproved_new_sl.front(); + const auto sl_back = data.unapproved_new_sl.back(); - if (new_shift_lines) { // clip from shift start index for visualize - clipByMinStartIdx(*new_shift_lines, shifted_path.path); - - int i = new_shift_lines->size() - 1; - for (; i > 0; i--) { - if (fabs(new_shift_lines->at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } - output.lateral_shift = new_shift_lines->at(i).getRelativeLength(); - output.start_distance_to_path_change = new_shift_lines->front().start_longitudinal; - output.finish_distance_to_path_change = new_shift_lines->back().end_longitudinal; + output.lateral_shift = sl.getRelativeLength(); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.lateral_shift > 0.0) { @@ -2341,7 +2360,7 @@ CandidateOutput AvoidanceModule::planCandidate() const return SteeringFactor::RIGHT; }); steering_factor_interface_ptr_->updateSteeringFactor( - {new_shift_lines->front().start, new_shift_lines->back().end}, + {sl_front.start, sl_back.end}, {output.start_distance_to_path_change, output.finish_distance_to_path_change}, SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); @@ -2379,23 +2398,19 @@ void AvoidanceModule::addShiftLineIfApproved(const AvoidLineArray & shift_lines) const size_t prev_size = path_shifter_.getShiftLinesSize(); addNewShiftLines(path_shifter_, shift_lines); + current_raw_shift_lines_ = avoidance_data_.unapproved_raw_sl; + // register original points for consistency registerRawShiftLines(shift_lines); - int i = shift_lines.size() - 1; - for (; i > 0; i--) { - if (fabs(shift_lines.at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } + const auto sl = getNonStraightShiftLine(shift_lines); + const auto sl_front = shift_lines.front(); + const auto sl_back = shift_lines.back(); - if (shift_lines.at(i).getRelativeLength() > 0.0) { - left_shift_array_.push_back({uuid_left_, shift_lines.front().start, shift_lines.back().end}); - } else if (shift_lines.at(i).getRelativeLength() < 0.0) { - right_shift_array_.push_back( - {uuid_right_, shift_lines.front().start, shift_lines.back().end}); + if (sl.getRelativeLength() > 0.0) { + left_shift_array_.push_back({uuid_left_, sl_front.start, sl_back.end}); + } else if (sl.getRelativeLength() < 0.0) { + right_shift_array_.push_back({uuid_right_, sl_front.start, sl_back.end}); } uuid_left_ = generateUUID(); @@ -2448,7 +2463,7 @@ void AvoidanceModule::addNewShiftLines( path_shifter.setShiftLines(future); } -boost::optional AvoidanceModule::findNewShiftLine( +AvoidLineArray AvoidanceModule::findNewShiftLine( const AvoidLineArray & candidates, const PathShifter & shifter) const { (void)shifter; @@ -2530,25 +2545,7 @@ boost::optional AvoidanceModule::findNewShiftLine( return {}; } -double AvoidanceModule::getEgoSpeed() const -{ - return std::abs(planner_data_->self_odometry->twist.twist.linear.x); -} - -double AvoidanceModule::getNominalAvoidanceEgoSpeed() const -{ - return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); -} -double AvoidanceModule::getSharpAvoidanceEgoSpeed() const -{ - return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); -} - -Point AvoidanceModule::getEgoPosition() const { return planner_data_->self_pose->pose.position; } - -PoseStamped AvoidanceModule::getEgoPose() const { return *(planner_data_->self_pose); } - -PoseStamped AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const +Pose AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const { const auto ego_pose = getEgoPose(); @@ -2557,43 +2554,17 @@ PoseStamped AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) } // un-shifted fot current ideal pose - const auto closest = findNearestIndex(prev_path.path.points, ego_pose.pose.position); + const auto closest = findNearestIndex(prev_path.path.points, ego_pose.position); - PoseStamped unshifted_pose = ego_pose; - unshifted_pose.pose.orientation = prev_path.path.points.at(closest).point.pose.orientation; + Pose unshifted_pose = ego_pose; + unshifted_pose.orientation = prev_path.path.points.at(closest).point.pose.orientation; - util::shiftPose(&unshifted_pose.pose, -prev_path.shift_length.at(closest)); - unshifted_pose.pose.orientation = ego_pose.pose.orientation; + util::shiftPose(&unshifted_pose, -prev_path.shift_length.at(closest)); + unshifted_pose.orientation = ego_pose.orientation; return unshifted_pose; } -double AvoidanceModule::getNominalAvoidanceDistance(const double shift_length) const -{ - const auto & p = parameters_; - const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); -} - -double AvoidanceModule::getSharpAvoidanceDistance(const double shift_length) const -{ - const auto & p = parameters_; - const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); -} - -double AvoidanceModule::getNominalPrepareDistance() const -{ - const auto & p = parameters_; - const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. - const auto nominal_distance = std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); - return nominal_distance + epsilon_m; -} - ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) const { DEBUG_PRINT("path_shifter: base shift = %f", getCurrentBaseShift()); @@ -2641,6 +2612,8 @@ void AvoidanceModule::updateData() if (prev_reference_.points.empty()) { prev_reference_ = avoidance_data_.reference_path; } + + fillShiftLine(avoidance_data_, debug_data_); } /* @@ -2882,16 +2855,6 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con return turn_signal_info; } -double AvoidanceModule::getCurrentShift() const -{ - return prev_output_.shift_length.at(findNearestIndex(prev_output_.path.points, getEgoPosition())); -} -double AvoidanceModule::getCurrentLinearShift() const -{ - return prev_linear_shift_path_.shift_length.at( - findNearestIndex(prev_linear_shift_path_.path.points, getEgoPosition())); -} - void AvoidanceModule::setDebugData( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { From 0c93a2c25320e1511ddcba52155491d809c44bc7 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Dec 2022 14:46:46 +0900 Subject: [PATCH 02/11] fix(mission_planner): set goal in shoulder lane (#2548) * fix(mission_planner): set goal in shoulder lane Signed-off-by: kosuke55 * fix map without shoulder lane Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/lanelet2_plugins/default_planner.cpp | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index b741bd4479763..55ed5a4cf57b0 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -272,6 +272,25 @@ bool DefaultPlanner::is_goal_valid( const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) { const auto logger = node_->get_logger(); + + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); + + // check if goal is in shoulder lanelet + lanelet::Lanelet closest_shoulder_lanelet; + if (lanelet::utils::query::getClosestLanelet( + shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { + if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { + const auto lane_yaw = + lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); + const auto goal_yaw = tf2::getYaw(goal.orientation); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + if (std::abs(angle_diff) < th_angle) { + return true; + } + } + } + lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { return false; @@ -298,8 +317,6 @@ bool DefaultPlanner::is_goal_valid( return false; } - const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); - if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); @@ -323,23 +340,6 @@ bool DefaultPlanner::is_goal_valid( return true; } - // check if goal is in shoulder lanelet - lanelet::Lanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { - return false; - } - // check if goal pose is in shoulder lane - if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); - const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); - if (std::abs(angle_diff) < th_angle) { - return true; - } - } return false; } From fea34b19001bb93fd127d07065c8b0b56fda48d0 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Dec 2022 15:51:02 +0900 Subject: [PATCH 03/11] feat(behavior_path_planner): plan shift pull over/out on curve (#2335) * feat(behavior_path_planner): plan shift pull over/out on curve Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp Co-authored-by: Fumiya Watanabe Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: Fumiya Watanabe Signed-off-by: kosuke55 * remove pull_out_finish_judge_buffer Signed-off-by: kosuke55 * remove commentout Signed-off-by: kosuke55 Signed-off-by: kosuke55 Co-authored-by: Fumiya Watanabe --- .../motion_utils/trajectory/trajectory.hpp | 38 +++ planning/behavior_path_planner/README.md | 48 ++- .../config/pull_out/pull_out.param.yaml | 3 +- .../config/pull_over/pull_over.param.yaml | 1 - .../scene_module/pull_out/pull_out_module.hpp | 1 + .../pull_out/pull_out_parameters.hpp | 3 +- .../scene_module/pull_out/shift_pull_out.hpp | 3 + .../scene_module/pull_over/goal_searcher.hpp | 9 +- .../pull_over/goal_searcher_base.hpp | 11 +- .../pull_over/pull_over_module.hpp | 17 +- .../pull_over/pull_over_parameters.hpp | 1 - .../pull_over/pull_over_planner_base.hpp | 1 + .../pull_over/shift_pull_over.hpp | 19 +- .../scene_module/pull_over/util.hpp | 9 +- .../behavior_path_planner/utilities.hpp | 4 + .../src/behavior_path_planner_node.cpp | 4 +- .../scene_module/pull_out/pull_out_module.cpp | 44 ++- .../scene_module/pull_out/shift_pull_out.cpp | 199 ++++++----- .../scene_module/pull_over/goal_searcher.cpp | 114 +++++-- .../pull_over/pull_over_module.cpp | 230 ++++++------- .../pull_over/shift_pull_over.cpp | 316 +++++++++++------- .../src/scene_module/pull_over/util.cpp | 38 +-- .../utils/geometric_parallel_parking.cpp | 10 + .../behavior_path_planner/src/utilities.cpp | 55 +++ .../trajectory_analyzer.hpp | 2 +- .../include/planning_debug_tools/util.hpp | 15 - 26 files changed, 723 insertions(+), 472 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 2a119fa02177d..f93e758c47332 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace motion_utils @@ -580,6 +581,43 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } +template +inline std::vector calcCurvature(const T & points) +{ + std::vector curvature_vec(points.size()); + + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); + curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3)); + } + curvature_vec.at(0) = curvature_vec.at(1); + curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); + + return curvature_vec; +} + +template +inline std::vector> calcCurvatureAndArcLength(const T & points) +{ + // Note that arclength is for the segment, not the sum. + std::vector> curvature_arc_length_vec; + curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); + const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); + const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + curvature_arc_length_vec.push_back(std::pair(curvature, arc_length)); + } + curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + + return curvature_arc_length_vec; +} + /** * @brief Calculate distance to the forward stop point from the given src index */ diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index d6bf2067c9cb7..b2a0f6085f532 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -271,15 +271,14 @@ The lateral jerk is searched for among the predetermined minimum and maximum val ###### Parameters for shift parking -| Name | Unit | Type | Description | Default value | -| :--------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | -| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | -| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | -| before_pull_over_straight_distance | [m] | double | straight line distance before pull over end point. a safe path should have a distance that includes this | 5.0 | +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | ##### **geometric parallel parking** @@ -362,13 +361,13 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of #### General parameters for pull_out -| Name | Unit | Type | Description | Default value | -| :--------------------------- | :---- | :----- | :------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| pull_out_finish_judge_buffer | [m] | double | threshold for finish judgment distance from pull out end point | 1.0 | +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | #### **Safe check with obstacles in shoulder lane** @@ -396,15 +395,14 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral ###### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| before_pull_out_straight_distance | [m] | double | distance before pull out start point | 0.0 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | ##### **geometric pull out** diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index 7a9a63f805d67..b092eb7fc8802 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -5,12 +5,11 @@ th_stopped_velocity: 0.01 th_stopped_time: 1.0 collision_check_margin: 1.0 - pull_out_finish_judge_buffer: 1.0 + collision_check_distance_from_end: 1.0 # shift pull out enable_shift_pull_out: true shift_pull_out_velocity: 2.0 pull_out_sampling_num: 4 - before_pull_out_straight_distance: 0.0 minimum_shift_pull_out_distance: 20.0 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 8e277959bc069..75b5facaaaafe 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -35,7 +35,6 @@ minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 after_pull_over_straight_distance: 5.0 - before_pull_over_straight_distance: 5.0 # parallel parking path enable_arc_forward_parking: true enable_arc_backward_parking: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 92cd395e159dd..9dbd977a1a95d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -114,6 +114,7 @@ class PullOutModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose); void planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose); + void generateStopPath(); void updatePullOutStatus(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index 96f76278e50df..21b66d1ad8457 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -26,12 +26,11 @@ struct PullOutParameters double th_stopped_velocity; double th_stopped_time; double collision_check_margin; - double pull_out_finish_judge_buffer; + double collision_check_distance_from_end; // shift pull out bool enable_shift_pull_out; double shift_pull_out_velocity; int pull_out_sampling_num; - double before_pull_out_straight_distance; double minimum_shift_pull_out_distance; double maximum_lateral_jerk; double minimum_lateral_jerk; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp index 4f43bb080fabd..7181f9bd8e9bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp @@ -49,6 +49,9 @@ class ShiftPullOut : public PullOutPlannerBase const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); + double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr); + std::shared_ptr lane_departure_checker_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp index 5ab414f0cb90e..f26d0e4cdebfe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp @@ -18,11 +18,14 @@ #include "behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp" #include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp" +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" + #include #include namespace behavior_path_planner { +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::LinearRing2d; class GoalSearcher : public GoalSearcherBase @@ -32,12 +35,14 @@ class GoalSearcher : public GoalSearcherBase const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); - std::vector search(const Pose & original_goal_pose) override; + GoalCandidates search(const Pose & original_goal_pose) override; + +private: + void createAreaPolygons(std::vector original_search_poses); bool checkCollision(const Pose & pose) const; bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; -private: LinearRing2d vehicle_footprint_{}; std::shared_ptr occupancy_grid_map_{}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp index 261e586c7c123..2672d3c659500 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp @@ -28,22 +28,21 @@ namespace behavior_path_planner { using geometry_msgs::msg::Pose; +using tier4_autoware_utils::MultiPolygon2d; struct GoalCandidate { Pose goal_pose{}; double distance_from_original_goal{0.0}; double lateral_offset{0.0}; + size_t id{0}; bool operator<(const GoalCandidate & other) const noexcept { - // compare in order of decreasing lateral offset. - if (std::abs(lateral_offset - other.lateral_offset) > std::numeric_limits::epsilon()) { - return lateral_offset < other.lateral_offset; - } return distance_from_original_goal < other.distance_from_original_goal; } }; +using GoalCandidates = std::vector; class GoalSearcherBase { @@ -56,11 +55,13 @@ class GoalSearcherBase planner_data_ = planner_data; } - virtual std::vector search(const Pose & original_goal_pose) = 0; + MultiPolygon2d getAreaPolygons() { return area_polygons_; } + virtual GoalCandidates search(const Pose & original_goal_pose) = 0; protected: PullOverParameters parameters_; std::shared_ptr planner_data_; + MultiPolygon2d area_polygons_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 7047c102b5bf2..831d185cb3889 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -65,6 +65,7 @@ struct PUllOverStatus bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; + std::optional stop_pose{}; }; class PullOverModule : public SceneModuleInterface @@ -103,9 +104,6 @@ class PullOverModule : public SceneModuleInterface PullOverPath shift_parking_path_; vehicle_info_util::VehicleInfo vehicle_info_; - const double pull_over_lane_length_{200.0}; - const double check_distance_{100.0}; - rclcpp::Subscription::SharedPtr occupancy_grid_sub_; rclcpp::Publisher::SharedPtr goal_pose_pub_; @@ -113,7 +111,7 @@ class PullOverModule : public SceneModuleInterface std::shared_ptr occupancy_grid_map_; Pose modified_goal_pose_; Pose refined_goal_pose_; - std::vector goal_candidates_; + GoalCandidates goal_candidates_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; std::deque odometry_buffer_; @@ -125,16 +123,11 @@ class PullOverModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; - PathWithLaneId getReferencePath() const; - PathWithLaneId generateStopPath() const; - Pose calcRefinedGoal() const; + Pose calcRefinedGoal(const Pose & goal_pose) const; ParallelParkingParameters getGeometricPullOverParameters() const; - bool isLongEnoughToParkingStart( - const PathWithLaneId & path, const Pose & parking_start_pose) const; - bool isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const; - double calcMinimumShiftPathDistance() const; std::pair calcDistanceToPathChange() const; + PathWithLaneId generateStopPath(); + PathWithLaneId generateEmergencyStopPath(); bool isStopped(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp index 5dc0876877e81..e0c7d5b86f9f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp @@ -56,7 +56,6 @@ struct PullOverParameters double pull_over_velocity; double pull_over_minimum_velocity; double after_pull_over_straight_distance; - double before_pull_over_straight_distance; // parallel parking bool enable_arc_forward_parking; bool enable_arc_backward_parking; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp index cfb4e488da1da..f6614595097ad 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp @@ -47,6 +47,7 @@ struct PullOverPath std::vector partial_paths{}; Pose start_pose{}; Pose end_pose{}; + std::vector debug_poses{}; }; class PullOverPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp index 19f723bd82385..3aaed89e2cca9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp @@ -41,21 +41,22 @@ class ShiftPullOver : public PullOverPlannerBase boost::optional plan(const Pose & goal_pose) override; protected: - PathWithLaneId generateRoadLaneReferencePath( - const lanelet::ConstLanelets & road_lanes, const Pose & shift_end_pose, - const double pull_over_distance) const; - PathWithLaneId generateShoulderLaneReferencePath( - const lanelet::ConstLanelets & shoulder_lanes, const Pose & shift_start_pose, - const Pose & goal_pose, const double shoulder_center_to_goal_distance) const; + PathWithLaneId generateReferencePath( + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; boost::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const Pose & shift_end_pose, const Pose & goal_pose, const double lateral_jerk, - const double road_center_to_goal_distance, const double shoulder_center_to_goal_distance, - const double shoulder_left_bound_to_goal_distance) const; + const Pose & goal_pose, const double lateral_jerk) const; bool hasEnoughDistance( const PathWithLaneId & path, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, const Pose & goal_pose, const double pull_over_distance) const; bool isSafePath(const PathWithLaneId & path) const; + static double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); + static std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s); + static std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval); LaneDepartureChecker lane_departure_checker_{}; std::shared_ptr occupancy_grid_map_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp index 343938898e7b9..2dbef9948f368 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp @@ -50,16 +50,15 @@ PredictedObjects filterObjectsByLateralDistance( const double distance_thresh, const bool filter_inside); // debug -Marker createPullOverAreaMarker( - const Pose & start_pose, const Pose & end_pose, const int32_t id, - const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, - const double vehicle_width, const std_msgs::msg::ColorRGBA & color); +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z); MarkerArray createPosesMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( - std::vector & goal_candidates, const std_msgs::msg::ColorRGBA & color); + GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); } // namespace pull_over_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 2de16855391ff..c1f9f05587a93 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -67,6 +67,7 @@ using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using route_handler::RouteHandler; +using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; @@ -358,6 +359,9 @@ PathPointWithLaneId insertStopPoint(double length, PathWithLaneId * path); double getSignedDistanceFromShoulderLeftBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose); +std::optional getSignedDistanceFromShoulderLeftBoundary( + const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, + const Pose & vehicle_pose); double getSignedDistanceFromRightBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5f5de72b26160..9638de981f81a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -449,7 +449,6 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.pull_over_velocity = dp("pull_over_velocity", 8.3); p.pull_over_minimum_velocity = dp("pull_over_minimum_velocity", 0.3); p.after_pull_over_straight_distance = dp("after_pull_over_straight_distance", 3.0); - p.before_pull_over_straight_distance = dp("before_pull_over_straight_distance", 3.0); // parallel parking p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); @@ -512,12 +511,11 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); p.th_stopped_time = dp("th_stopped_time", 1.0); p.collision_check_margin = dp("collision_check_margin", 1.0); - p.pull_out_finish_judge_buffer = dp("pull_out_finish_judge_buffer", 1.0); + p.collision_check_distance_from_end = dp("collision_check_distance_from_end", 3.0); // shift pull out p.enable_shift_pull_out = dp("enable_shift_pull_out", true); p.shift_pull_out_velocity = dp("shift_pull_out_velocity", 8.3); p.pull_out_sampling_num = dp("pull_out_sampling_num", 4); - p.before_pull_out_straight_distance = dp("before_pull_out_straight_distance", 3.0); p.minimum_shift_pull_out_distance = dp("minimum_shift_pull_out_distance", 20.0); p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index e89ad45cdadd9..79c1a84fafce3 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -31,6 +31,7 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; + namespace behavior_path_planner { PullOutModule::PullOutModule( @@ -173,6 +174,7 @@ BehaviorModuleOutput PullOutModule::plan() BehaviorModuleOutput output; if (!status_.is_safe) { + RCLCPP_INFO(getLogger(), "not found safe path"); return output; } @@ -370,6 +372,7 @@ void PullOutModule::planWithPriorityOnEfficientPath( const std::vector & start_pose_candidates, const Pose & goal_pose) { status_.is_safe = false; + status_.planner_type = PlannerType::NONE; // plan with each planner for (const auto & planner : pull_out_planners_) { @@ -401,6 +404,7 @@ void PullOutModule::planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose) { status_.is_safe = false; + status_.planner_type = PlannerType::NONE; for (size_t i = 0; i < start_pose_candidates.size(); ++i) { // pull out start pose is current_pose @@ -428,6 +432,33 @@ void PullOutModule::planWithPriorityOnShortBackDistance( } } +void PullOutModule::generateStopPath() +{ + const auto & current_pose = planner_data_->self_pose->pose; + constexpr double dummy_path_distance = 1.0; + const auto & moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); + + // convert Pose to PathPointWithLaneId with 0 velocity. + auto toPathPointWithLaneId = [this](const Pose & pose) { + PathPointWithLaneId p{}; + p.point.pose = pose; + p.point.longitudinal_velocity_mps = 0.0; + lanelet::Lanelet closest_shoulder_lanelet; + lanelet::utils::query::getClosestLanelet( + planner_data_->route_handler->getShoulderLanelets(), pose, &closest_shoulder_lanelet); + p.lane_ids.push_back(closest_shoulder_lanelet.id()); + return p; + }; + + PathWithLaneId path{}; + path.points.push_back(toPathPointWithLaneId(current_pose)); + path.points.push_back(toPathPointWithLaneId(moved_pose)); + + status_.pull_out_path.partial_paths.push_back(path); + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; +} + void PullOutModule::updatePullOutStatus() { const auto & route_handler = planner_data_->route_handler; @@ -462,12 +493,14 @@ void PullOutModule::updatePullOutStatus() getLogger(), "search_priority should be efficient_path or short_back_distance, but %s is given.", parameters_.search_priority.c_str()); + throw std::domain_error("[pull_out] invalid search_priority"); } if (!status_.is_safe) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Not found safe pull out path"); - status_.is_safe = false; - return; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); + status_.back_finished = true; // no need to drive backward + generateStopPath(); } checkBackFinished(); @@ -550,8 +583,7 @@ bool PullOutModule::hasFinishedPullOut() const lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); // has passed pull out end point - return arclength_current.length - arclength_pull_out_end.length > - parameters_.pull_out_finish_judge_buffer; + return arclength_current.length - arclength_pull_out_end.length > 0.0; } void PullOutModule::checkBackFinished() @@ -633,7 +665,7 @@ TurnSignalInfo PullOutModule::calcTurnSignalInfo() const // pull out path does not overlap const double distance_from_end = motion_utils::calcSignedArcLength( path.points, status_.pull_out_path.end_pose.position, current_pose.position); - if (distance_from_end < parameters_.pull_out_finish_judge_buffer) { + if (distance_from_end < 0.0) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index b72558260ed75..d16d69839ba36 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -47,10 +47,6 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) return boost::none; } - const auto drivable_lanes = - util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto lanes = util::transformToLanelets(drivable_lanes); - // find candidate paths auto pull_out_paths = calcPullOutPaths( *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters, @@ -71,15 +67,33 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // check lane_departure and collision with path between pull_out_start to pull_out_end PathWithLaneId path_start_to_end{}; { - const auto pull_out_start_idx = findNearestIndex(shift_path.points, start_pose); - const auto pull_out_end_idx = findNearestIndex(shift_path.points, pull_out_path.end_pose); + const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); + + // calculate collision check end idx + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + shift_path.points, pull_out_path.end_pose.position, + parameters_.collision_check_distance_from_end); + + if (collision_check_end_pose) { + return findNearestIndex(shift_path.points, collision_check_end_pose->position); + } else { + return shift_path.points.size() - 1; + } + }); path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + *pull_out_start_idx, - shift_path.points.begin() + *pull_out_end_idx + 1); + path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + collision_check_end_idx + 1); } // check lane departure - if (lane_departure_checker_->checkPathWillLeaveLane(lanes, path_start_to_end)) { + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + if (lane_departure_checker_->checkPathWillLeaveLane( + util::transformToLanelets(expanded_lanes), path_start_to_end)) { continue; } @@ -108,7 +122,7 @@ std::vector ShiftPullOut::calcPullOutPaths( const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter) { - std::vector candidate_paths; + std::vector candidate_paths{}; if (road_lanes.empty() || shoulder_lanes.empty()) { return candidate_paths; @@ -117,7 +131,6 @@ std::vector ShiftPullOut::calcPullOutPaths( // rename parameter const double backward_path_length = common_parameter.backward_path_length; const double shift_pull_out_velocity = parameter.shift_pull_out_velocity; - const double before_pull_out_straight_distance = parameter.before_pull_out_straight_distance; const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance; const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; @@ -127,64 +140,59 @@ std::vector ShiftPullOut::calcPullOutPaths( for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; lateral_jerk += jerk_resolution) { - PathShifter path_shifter; - const double distance_to_road_center = getArcCoordinates(road_lanes, start_pose).distance; + // lateral distance from road center to start pose + const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; + + PathWithLaneId road_lane_reference_path{}; + { + const auto arc_position = getArcCoordinates(road_lanes, start_pose); + const double s_start = std::max(arc_position.length - backward_path_length, 0.0); + const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); + double s_end = arc_position_goal.length; + road_lane_reference_path = util::resamplePathWithSpline( + route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0); + } + PathShifter path_shifter{}; + path_shifter.setPath(road_lane_reference_path); + + // calculate after/before shifted pull out distance const double pull_out_distance = std::max( - path_shifter.calcLongitudinalDistFromJerk( - abs(distance_to_road_center), lateral_jerk, shift_pull_out_velocity), + PathShifter::calcLongitudinalDistFromJerk( + abs(shift_length), lateral_jerk, shift_pull_out_velocity), minimum_shift_pull_out_distance); + const size_t shift_start_idx = + findNearestIndex(road_lane_reference_path.points, start_pose.position); + PathWithLaneId road_lane_reference_path_from_ego = road_lane_reference_path; + road_lane_reference_path_from_ego.points.erase( + road_lane_reference_path_from_ego.points.begin(), + road_lane_reference_path_from_ego.points.begin() + shift_start_idx); + // before means distance on road lane + const double before_shifted_pull_out_distance = calcBeforeShiftedArcLength( + road_lane_reference_path_from_ego, pull_out_distance, shift_length); // check has enough distance - const double pull_out_total_distance = before_pull_out_straight_distance + pull_out_distance; const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); if (!hasEnoughDistance( - pull_out_total_distance, road_lanes, start_pose, is_in_goal_route_section, goal_pose)) { + before_shifted_pull_out_distance, road_lanes, start_pose, is_in_goal_route_section, + goal_pose)) { continue; } - PathWithLaneId shoulder_reference_path; - { - const auto arc_position = getArcCoordinates(shoulder_lanes, start_pose); - const double s_start = arc_position.length - backward_path_length; - double s_end = arc_position.length + before_pull_out_straight_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - shoulder_reference_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end); - - // lateral shift to start_pose - for (auto & p : shoulder_reference_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, arc_position.distance, 0); - } - } - - PathWithLaneId road_lane_reference_path; - { - const lanelet::ArcCoordinates arc_position_shift = - getArcCoordinates(road_lanes, shoulder_reference_path.points.back().point.pose); - const lanelet::ArcCoordinates arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - - double s_start = arc_position_shift.length; - double s_end = arc_position_goal.length; - road_lane_reference_path = route_handler.getCenterLinePath(road_lanes, s_start, s_end); - } - path_shifter.setPath(road_lane_reference_path); - - // get shift point start/end - const auto shift_line_start = shoulder_reference_path.points.back(); - const auto shift_line_end = [&]() { + // get shift end pose + const auto shift_end_pose = std::invoke([&]() { const auto arc_position_shift_start = - lanelet::utils::getArcCoordinates(road_lanes, shift_line_start.point.pose); - const double s_start = arc_position_shift_start.length + pull_out_distance; + lanelet::utils::getArcCoordinates(road_lanes, start_pose); + const double s_start = arc_position_shift_start.length + before_shifted_pull_out_distance; const double s_end = s_start + std::numeric_limits::epsilon(); const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true); - return path.points.front(); - }(); - - ShiftLine shift_line; - { - shift_line.start = shift_line_start.point.pose; - shift_line.end = shift_line_end.point.pose; - shift_line.end_shift_length = distance_to_road_center; - } + return path.points.front().point.pose; + }); + + // create shift line + ShiftLine shift_line{}; + shift_line.start = start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_length; path_shifter.addShiftLine(shift_line); // offset front side @@ -194,43 +202,28 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } - const auto pull_out_end_idx = - findNearestIndex(shifted_path.path.points, shift_line_end.point.pose); - const auto goal_idx = findNearestIndex(shifted_path.path.points, goal_pose); - - if (pull_out_end_idx && goal_idx) { - // set velocity - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *pull_out_end_idx) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); - continue; - } else if (i > *goal_idx) { - point.point.longitudinal_velocity_mps = 0.0; - continue; - } + // set velocity + const size_t pull_out_end_idx = + findNearestIndex(shifted_path.path.points, shift_end_pose.position); + const size_t goal_idx = findNearestIndex(shifted_path.path.points, goal_pose.position); + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < pull_out_end_idx) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); + continue; + } else if (i >= goal_idx) { + point.point.longitudinal_velocity_mps = 0.0; + continue; } - - PullOutPath candidate_path; - const auto combined_path = combineReferencePath(shoulder_reference_path, shifted_path.path); - candidate_path.partial_paths.push_back(util::resamplePathWithSpline(combined_path, 1.0)); - candidate_path.start_pose = shift_line.start; - candidate_path.end_pose = shift_line.end; - - // add goal pose because resampling removes it - // but this point will be removed by SmoothGoalConnection again - PathPointWithLaneId goal_path_point = shifted_path.path.points.back(); - goal_path_point.point.pose = goal_pose; - goal_path_point.point.longitudinal_velocity_mps = 0.0; - candidate_path.partial_paths.front().points.push_back(goal_path_point); - candidate_paths.push_back(candidate_path); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "lane change end idx not found on target path."); - continue; } + + // add shifted path to candidates + PullOutPath candidate_path; + candidate_path.partial_paths.push_back(shifted_path.path); + candidate_path.start_pose = shift_line.start; + candidate_path.end_pose = shift_line.end; + candidate_paths.push_back(candidate_path); } return candidate_paths; @@ -255,4 +248,26 @@ bool ShiftPullOut::hasEnoughDistance( return true; } +double ShiftPullOut::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) +{ + double before_arc_length{0.0}; + double after_arc_length{0.0}; + + for (const auto & [k, segment_length] : motion_utils::calcCurvatureAndArcLength(path.points)) { + // after shifted segment length + const double after_segment_length = + k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + before_arc_length += k < 0 ? offset / (1 - k * dr) : offset * (1 + k * dr); + break; + } + before_arc_length += segment_length; + after_arc_length += after_segment_length; + } + + return before_arc_length; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index af3abb2d10e48..dea04a5ab85fe 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -14,7 +14,9 @@ #include "behavior_path_planner/scene_module/pull_over/goal_searcher.hpp" +#include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/pull_over/util.hpp" +#include "lanelet2_extension/utility/utilities.hpp" #include @@ -36,53 +38,78 @@ GoalSearcher::GoalSearcher( { } -std::vector GoalSearcher::search(const Pose & original_goal_pose) +GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) { - std::vector goal_candidates{}; + GoalCandidates goal_candidates{}; - const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); + const auto & route_handler = planner_data_->route_handler; + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double forward_length = parameters_.forward_goal_search_length; + const double backward_length = parameters_.backward_goal_search_length; + const double margin_from_boundary = parameters_.margin_from_boundary; + const double lateral_offset_interval = parameters_.lateral_offset_interval; + const double max_lateral_offset = parameters_.max_lateral_offset; + + const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*route_handler); auto lanes = util::getExtendedCurrentLanes(planner_data_); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); + const auto goal_arc_coords = + lanelet::utils::getArcCoordinates(pull_over_lanes, original_goal_pose); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + auto center_line_path = util::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + parameters_.goal_search_interval); + const auto shoulder_lane_objects = util::filterObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); - for (double dx = -parameters_.backward_goal_search_length; - dx <= parameters_.forward_goal_search_length; dx += parameters_.goal_search_interval) { - // search goal_pose in lateral direction + std::vector original_search_poses{}; + for (size_t goal_id = 0; goal_id < center_line_path.points.size(); ++goal_id) { + const auto & center_pose = center_line_path.points.at(goal_id).point.pose; + + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( + pull_over_lanes, vehicle_footprint_, center_pose); + if (!distance_from_left_bound) continue; + + const double offset_from_center_line = distance_from_left_bound.value() + margin_from_boundary; + const Pose original_search_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; + // search goal_pose in lateral direction bool found_lateral_no_collision_pose = false; double lateral_offset = 0.0; - for (double dy = 0; dy <= parameters_.max_lateral_offset; - dy += parameters_.lateral_offset_interval) { + for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_goal_pose, dx, -dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, -dy, 0); - const auto & transformed_vehicle_footprint = + const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { continue; } - if (checkCollision(search_pose)) { continue; } - // if finding objects near the search pose, + // if finding objects or detecting lane departure // shift search_pose in lateral direction one more - // because collision may be detected on other path points - if (dy > 0) { - search_pose = calcOffsetPose(search_pose, 0, -parameters_.lateral_offset_interval, 0); + // for avoiding them on other path points. + if (dy > 0 && dy < max_lateral_offset) { + search_pose = calcOffsetPose(search_pose, 0, -lateral_offset_interval, 0); } found_lateral_no_collision_pose = true; break; } - if (!found_lateral_no_collision_pose) continue; + if (!found_lateral_no_collision_pose) { + continue; + } constexpr bool filter_inside = true; const auto target_objects = pull_over_utils::filterObjectsByLateralDistance( - search_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + search_pose, vehicle_width, shoulder_lane_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(search_pose, target_objects)) { continue; @@ -91,13 +118,14 @@ std::vector GoalSearcher::search(const Pose & original_goal_pose) GoalCandidate goal_candidate{}; goal_candidate.goal_pose = search_pose; goal_candidate.lateral_offset = lateral_offset; + goal_candidate.id = goal_id; // use longitudinal_distance as distance_from_original_goal - // TODO(kosuke55): use arc length for curve lane - goal_candidate.distance_from_original_goal = - std::abs(inverseTransformPose(search_pose, original_goal_pose).position.x); - + goal_candidate.distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( + center_line_path.points, original_goal_pose.position, search_pose.position)); goal_candidates.push_back(goal_candidate); } + createAreaPolygons(original_search_poses); + // Sort with distance from original goal std::sort(goal_candidates.begin(), goal_candidates.end()); @@ -169,4 +197,48 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( return false; } +void GoalSearcher::createAreaPolygons(std::vector original_search_poses) +{ + using tier4_autoware_utils::MultiPolygon2d; + using tier4_autoware_utils::Point2d; + using tier4_autoware_utils::Polygon2d; + + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + const double max_lateral_offset = parameters_.max_lateral_offset; + + const auto appendPointToPolygon = + [](Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { + Point2d point{}; + point.x() = geom_point.x; + point.y() = geom_point.y; + boost::geometry::append(polygon.outer(), point); + }; + + boost::geometry::clear(area_polygons_); + for (const auto p : original_search_poses) { + Polygon2d footprint{}; + const Point p_left_front = calcOffsetPose(p, base_link2front, vehicle_width / 2, 0).position; + appendPointToPolygon(footprint, p_left_front); + + const Point p_right_front = + calcOffsetPose(p, base_link2front, -vehicle_width / 2 - max_lateral_offset, 0).position; + appendPointToPolygon(footprint, p_right_front); + + const Point p_right_back = + calcOffsetPose(p, -base_link2rear, -vehicle_width / 2 - max_lateral_offset, 0).position; + appendPointToPolygon(footprint, p_right_back); + + const Point p_left_back = calcOffsetPose(p, -base_link2rear, vehicle_width / 2, 0).position; + appendPointToPolygon(footprint, p_left_back); + + appendPointToPolygon(footprint, p_left_front); + + MultiPolygon2d current_result{}; + boost::geometry::union_(footprint, area_polygons_, current_result); + area_polygons_ = current_result; + } +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 54b78686b1e2a..6544c581d2169 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/util/create_vehicle_footprint.hpp" #include "behavior_path_planner/utilities.hpp" +#include #include #include #include @@ -32,6 +33,7 @@ #include #include +using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using nav_msgs::msg::OccupancyGrid; @@ -166,7 +168,7 @@ void PullOverModule::onEntry() std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); // Use refined goal as modified goal when disabling goal research - refined_goal_pose_ = calcRefinedGoal(); + refined_goal_pose_ = calcRefinedGoal(planner_data_->route_handler->getGoalPose()); if (!parameters_.enable_goal_research) { goal_candidates_.clear(); GoalCandidate goal_candidate{}; @@ -246,24 +248,47 @@ bool PullOverModule::isExecutionRequested() const bool PullOverModule::isExecutionReady() const { return true; } -Pose PullOverModule::calcRefinedGoal() const +Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const { - lanelet::ConstLanelet goal_lane; - Pose goal_pose = planner_data_->route_handler->getGoalPose(); - lanelet::Lanelet closest_shoulder_lanelet; lanelet::utils::query::getClosestLanelet( planner_data_->route_handler->getShoulderLanelets(), goal_pose, &closest_shoulder_lanelet); - const Pose center_pose = - lanelet::utils::getClosestCenterPose(closest_shoulder_lanelet, goal_pose.position); - - const double distance_to_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( - planner_data_->route_handler->getShoulderLanelets(), center_pose); - const double offset_from_center_line = distance_to_left_bound + - planner_data_->parameters.vehicle_width / 2 + - parameters_.margin_from_boundary; - const Pose refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + // calc closest center line pose + Pose center_pose; + { + // find position + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); + const auto segment = lanelet::utils::getClosestSegment( + lanelet::utils::to2D(lanelet_point), closest_shoulder_lanelet.centerline()); + const auto p1 = segment.front().basicPoint(); + const auto p2 = segment.back().basicPoint(); + const auto direction_vector = (p2 - p1).normalized(); + const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const double s = direction_vector.dot(p1_to_goal); + const auto refined_point = p1 + direction_vector * s; + + center_pose.position.x = refined_point.x(); + center_pose.position.y = refined_point.y(); + center_pose.position.z = refined_point.z(); + + // find orientation + const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + center_pose.orientation = tf2::toMsg(tf_quat); + } + + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( + planner_data_->route_handler->getShoulderLanelets(), vehicle_footprint_, center_pose); + if (!distance_from_left_bound) { + RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); + return goal_pose; + } + + const double offset_from_center_line = + distance_from_left_bound.value() + parameters_.margin_from_boundary; + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; } @@ -280,31 +305,6 @@ BT::NodeStatus PullOverModule::updateState() return current_state_; } -bool PullOverModule::isLongEnoughToParkingStart( - const PathWithLaneId & path, const Pose & parking_start_pose) const -{ - const auto dist_to_parking_start_pose = calcSignedArcLength( - path.points, planner_data_->self_pose->pose, parking_start_pose.position, - std::numeric_limits::max(), M_PI_2); - if (!dist_to_parking_start_pose) return false; - - const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - - // once stopped, it cannot start again if start_pose is close. - // so need enough distance to restart - const double eps_vel = 0.01; - // dist to restart should be less than decide_path_distance. - // otherwise, the goal would change immediately after departure. - const double dist_to_restart = parameters_.decide_path_distance / 2; - if (std::abs(current_vel) < eps_vel && *dist_to_parking_start_pose < dist_to_restart) { - return false; - } - - return *dist_to_parking_start_pose > current_to_stop_distance; -} - bool PullOverModule::planWithEfficientPath() { for (const auto & planner : pull_over_planners_) { @@ -427,30 +427,32 @@ BehaviorModuleOutput PullOverModule::plan() // Decelerate before the minimum shift distance from the goal search area. if (status_.is_safe) { auto & first_path = status_.pull_over_path.partial_paths.front(); - const auto arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); - const Pose search_start_pose = calcOffsetPose( - refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); + const auto search_start_pose = calcLongitudinalOffsetPose( + first_path.points, refined_goal_pose_.position, + -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); + const Pose deceleration_pose = + search_start_pose ? *search_start_pose : first_path.points.front().point.pose; + constexpr double deceleration_buffer = 15.0; first_path = util::setDecelerationVelocity( - first_path, parameters_.pull_over_velocity, search_start_pose, - -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); + first_path, parameters_.pull_over_velocity, deceleration_pose, -deceleration_buffer, + parameters_.deceleration_interval); } - } - // generate drivable area for each partial path - for (size_t i = status_.current_path_idx; i < status_.pull_over_path.partial_paths.size(); ++i) { - auto & path = status_.pull_over_path.partial_paths.at(i); - const auto p = planner_data_->parameters; - const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); - const auto lane = util::expandLanelets( - shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); - util::generateDrivableArea(path, lane, p.vehicle_length, planner_data_); + // generate drivable area for each partial path + for (auto & path : status_.pull_over_path.partial_paths) { + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); + const auto expanded_lanes = util::expandLanelets( + shorten_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + util::generateDrivableArea( + path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + } } BehaviorModuleOutput output; // safe: use pull over path if (status_.is_safe) { + status_.stop_pose.reset(); output.path = std::make_shared(getCurrentPath()); path_candidate_ = std::make_shared(getFullPath()); } else { @@ -523,7 +525,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() updateOccupancyGrid(); BehaviorModuleOutput out; plan(); // update status_ - out.path = std::make_shared(getReferencePath()); + out.path = std::make_shared(generateStopPath()); path_candidate_ = status_.is_safe ? std::make_shared(getFullPath()) : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); @@ -566,7 +568,7 @@ void PullOverModule::setParameters(const PullOverParameters & parameters) parameters_ = parameters; } -PathWithLaneId PullOverModule::getReferencePath() const +PathWithLaneId PullOverModule::generateStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_pose->pose; @@ -585,12 +587,13 @@ PathWithLaneId PullOverModule::getReferencePath() const route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); // if not approved, stop parking start position or goal search start position. - const auto refined_goal_arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); - const Pose search_start_pose = calcOffsetPose( - refined_goal_pose_, -parameters_.backward_goal_search_length, - -refined_goal_arc_coordinates.distance, 0); - const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : search_start_pose; + const auto search_start_pose = calcLongitudinalOffsetPose( + reference_path.points, refined_goal_pose_.position, + -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); + if (!status_.is_safe && !search_start_pose) { + return generateEmergencyStopPath(); + } + const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : *search_start_pose; // if stop pose is behind current pose, stop as soon as possible const size_t ego_idx = findEgoIndex(reference_path.points); @@ -600,48 +603,58 @@ PathWithLaneId PullOverModule::getReferencePath() const const double ego_to_stop_distance = calcSignedArcLength( reference_path.points, current_pose.position, ego_idx, stop_pose.position, stop_idx); if (ego_to_stop_distance < 0.0) { - return generateStopPath(); + return generateEmergencyStopPath(); } // slow down for turn signal, insert stop point to stop_pose reference_path = util::setDecelerationVelocityForTurnSignal( reference_path, stop_pose, planner_data_->parameters.turn_signal_search_time); + status_.stop_pose = stop_pose; // slow down before the search area. + constexpr double deceleration_buffer = 15.0; reference_path = util::setDecelerationVelocity( - reference_path, parameters_.pull_over_velocity, search_start_pose, - -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); + reference_path, parameters_.pull_over_velocity, *search_start_pose, -deceleration_buffer, + parameters_.deceleration_interval); + // generate drivable area const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); - const auto lanes = util::expandLanelets( + const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); util::generateDrivableArea( - reference_path, lanes, common_parameters.vehicle_length, planner_data_); + reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } -PathWithLaneId PullOverModule::generateStopPath() const +PathWithLaneId PullOverModule::generateEmergencyStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_pose->pose; const auto & common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + // generate stop reference path const auto s_current = lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + // calc minimum stop distance under maximum deceleration + const double min_stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + + // set stop point + const auto stop_idx = + motion_utils::insertStopPoint(current_pose, min_stop_distance, stop_path.points); + if (stop_idx) { + status_.stop_pose = stop_path.points.at(*stop_idx).point.pose; + } + // set deceleration velocity const size_t ego_idx = findEgoIndex(stop_path.points); - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - motion_utils::insertStopPoint(current_pose, current_to_stop_distance, stop_path.points); - for (auto & point : stop_path.points) { auto & p = point.point; const size_t target_idx = findFirstNearestSegmentIndexWithSoftConstraints( @@ -652,7 +665,7 @@ PathWithLaneId PullOverModule::generateStopPath() const if (0.0 < distance_to_target) { p.longitudinal_velocity_mps = std::clamp( static_cast( - current_vel * (current_to_stop_distance - distance_to_target) / current_to_stop_distance), + current_vel * (min_stop_distance - distance_to_target) / min_stop_distance), 0.0f, p.longitudinal_velocity_mps); } else { p.longitudinal_velocity_mps = @@ -660,13 +673,14 @@ PathWithLaneId PullOverModule::generateStopPath() const } } + // generate drivable area const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto shorten_lanes = util::cutOverlappedLanes(stop_path, drivable_lanes); - const auto lanes = util::expandLanelets( + const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - - util::generateDrivableArea(stop_path, lanes, common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + stop_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return stop_path; } @@ -700,40 +714,6 @@ PathWithLaneId PullOverModule::getFullPath() const return path; } -double PullOverModule::calcMinimumShiftPathDistance() const -{ - const double maximum_jerk = parameters_.maximum_lateral_jerk; - const double pull_over_velocity = parameters_.pull_over_velocity; - const auto current_pose = planner_data_->self_pose->pose; - const double distance_after_pull_over = parameters_.after_pull_over_straight_distance; - const double distance_before_pull_over = parameters_.before_pull_over_straight_distance; - const auto & route_handler = planner_data_->route_handler; - - double distance_to_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( - route_handler->getShoulderLanelets(), current_pose); - double offset_from_center_line = distance_to_left_bound + - planner_data_->parameters.vehicle_width / 2 + - parameters_.margin_from_boundary; - - // calculate minimum pull over distance at pull over velocity, maximum jerk and side offset - const double pull_over_distance_min = PathShifter::calcLongitudinalDistFromJerk( - abs(offset_from_center_line), maximum_jerk, pull_over_velocity); - const double pull_over_total_distance_min = - distance_after_pull_over + pull_over_distance_min + distance_before_pull_over; - - return pull_over_total_distance_min; -} - -bool PullOverModule::isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer) const -{ - const auto current_pose = planner_data_->self_pose->pose; - const double distance_to_goal = - std::abs(util::getSignedDistance(current_pose, goal_pose, lanelets)); - - return distance_to_goal > calcMinimumShiftPathDistance() + buffer; -} - bool PullOverModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); @@ -815,6 +795,7 @@ void PullOverModule::setDebugData() using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createMarkerColor; const auto add = [this](const MarkerArray & added) { @@ -823,17 +804,12 @@ void PullOverModule::setDebugData() if (parameters_.enable_goal_research) { // Visualize pull over areas - const Pose start_pose = - calcOffsetPose(refined_goal_pose_, -parameters_.backward_goal_search_length, 0, 0); - const Pose end_pose = - calcOffsetPose(refined_goal_pose_, parameters_.forward_goal_search_length, 0, 0); const auto header = planner_data_->route_handler->getRouteHeader(); const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const auto p = planner_data_->parameters; - debug_marker_.markers.push_back(pull_over_utils::createPullOverAreaMarker( - start_pose, end_pose, 0, header, p.base_link2front, p.base_link2rear, p.vehicle_width, - color)); + const double z = refined_goal_pose_.position.z; + add(pull_over_utils::createPullOverAreaMarkerArray( + goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates add(pull_over_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); @@ -844,9 +820,23 @@ void PullOverModule::setDebugData() add(createPoseMarkerArray( status_.pull_over_path.start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.9, 0.9, 0.3)); + status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); } + + // Visualize debug poses + const auto & debug_poses = status_.pull_over_path.debug_poses; + for (size_t i = 0; i < debug_poses.size(); ++i) { + add(createPoseMarkerArray( + debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); + } + + // Visualize stop pose + if (status_.stop_pose) { + add(createStopVirtualWallMarker( + *status_.stop_pose, "pull_over", clock_->now(), 0, + planner_data_->parameters.base_link2front)); + } } void PullOverModule::printParkingPositionError() const diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index 7307ab76278af..69ab6a56b0cf6 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -34,61 +34,25 @@ ShiftPullOver::ShiftPullOver( occupancy_grid_map_{occupancy_grid_map} { } - boost::optional ShiftPullOver::plan(const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; - const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; const double min_jerk = parameters_.minimum_lateral_jerk; const double max_jerk = parameters_.maximum_lateral_jerk; const int pull_over_sampling_num = parameters_.pull_over_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / pull_over_sampling_num; + // get road and shoulder lanes const auto road_lanes = util::getExtendedCurrentLanes(planner_data_); const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler); if (road_lanes.empty() || shoulder_lanes.empty()) { return {}; } - const auto goal_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); - - // calculate the pose whose longitudinal position is shift end - // and whose lateral position is center line - const auto shift_end_shoulder_center_pose = std::invoke([&]() { - const double s_start = std::max( - goal_shoulder_arc_coords.length - after_pull_over_straight_distance, 0.0); // shift end - const double s_end = s_start + std::numeric_limits::epsilon(); - const auto shoulder_lane_path = - route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end, true); - return shoulder_lane_path.points.front().point.pose; - }); - - // calculate shift end pose - const double shoulder_left_bound_to_center_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, shift_end_shoulder_center_pose); - const double shoulder_left_bound_to_goal_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, goal_pose); - const double shoulder_center_to_goal_distance = - -shoulder_left_bound_to_center_distance + shoulder_left_bound_to_goal_distance; - const auto shift_end_pose = tier4_autoware_utils::calcOffsetPose( - shift_end_shoulder_center_pose, 0, shoulder_center_to_goal_distance, 0); - - // calculate lateral distances - lanelet::ConstLanelet goal_closest_road_lane{}; - lanelet::utils::query::getClosestLanelet(road_lanes, goal_pose, &goal_closest_road_lane); - const auto road_center_pose = - lanelet::utils::getClosestCenterPose(goal_closest_road_lane, goal_pose.position); - const double shoulder_left_bound_to_road_center = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, road_center_pose); - const double road_center_to_goal_distance = - -shoulder_left_bound_to_road_center + shoulder_left_bound_to_goal_distance; - + // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { - const auto pull_over_path = generatePullOverPath( - road_lanes, shoulder_lanes, shift_end_pose, goal_pose, lateral_jerk, - road_center_to_goal_distance, shoulder_center_to_goal_distance, - shoulder_left_bound_to_goal_distance); + const auto pull_over_path = + generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -96,9 +60,8 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) return {}; } -PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( - const lanelet::ConstLanelets & road_lanes, const Pose & shift_end_pose, - const double pull_over_distance) const +PathWithLaneId ShiftPullOver::generateReferencePath( + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const { const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_pose->pose; @@ -108,14 +71,10 @@ PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( const auto current_road_arc_coords = lanelet::utils::getArcCoordinates(road_lanes, current_pose); const double s_start = current_road_arc_coords.length - backward_path_length; - const auto shift_end_road_arc_coords = - lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose); - double s_end = shift_end_road_arc_coords.length - pull_over_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); + const double s_end = std::max( + lanelet::utils::getArcCoordinates(road_lanes, end_pose).length, + s_start + std::numeric_limits::epsilon()); auto road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); - // resample road straight path and shift source path respectively - road_lane_reference_path = - util::resamplePathWithSpline(road_lane_reference_path, resample_interval_); // decelerate velocity linearly to minimum pull over velocity // (or keep original velocity if it is lower than pull over velocity) @@ -133,115 +92,134 @@ PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( return road_lane_reference_path; } -PathWithLaneId ShiftPullOver::generateShoulderLaneReferencePath( - const lanelet::ConstLanelets & shoulder_lanes, const Pose & shift_start_pose, - const Pose & goal_pose, const double shoulder_center_to_goal_distance) const -{ - const auto & route_handler = planner_data_->route_handler; - - const auto shift_start_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, shift_start_pose); - const double s_start = shift_start_shoulder_arc_coords.length; - const auto goal_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); - const double s_end = goal_shoulder_arc_coords.length; - auto shoulder_lane_reference_path = - route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end); - // offset to goal line - for (auto & p : shoulder_lane_reference_path.points) { - p.point.pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, shoulder_center_to_goal_distance, 0); - } - shoulder_lane_reference_path = - util::resamplePathWithSpline(shoulder_lane_reference_path, resample_interval_); - - return shoulder_lane_reference_path; -} - boost::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const Pose & shift_end_pose, const Pose & goal_pose, const double lateral_jerk, - const double road_center_to_goal_distance, const double shoulder_center_to_goal_distance, - const double shoulder_left_bound_to_goal_distance) const + const Pose & goal_pose, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; + const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; + + const Pose shift_end_pose = + tier4_autoware_utils::calcOffsetPose(goal_pose, -after_pull_over_straight_distance, 0, 0); + + // generate road lane reference path to shift end + const auto road_lane_reference_path_to_shift_end = util::resamplePathWithSpline( + generateReferencePath(road_lanes, shift_end_pose), resample_interval_); + + // calculate shift length + const Pose & shift_end_pose_road_lane = + road_lane_reference_path_to_shift_end.points.back().point.pose; + const double shift_end_road_to_target_distance = + tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + .y; - // generate road lane reference path and get shift start pose + // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( - road_center_to_goal_distance, lateral_jerk, pull_over_velocity); - const auto road_lane_reference_path = - generateRoadLaneReferencePath(road_lanes, shift_end_pose, pull_over_distance); - if (road_lane_reference_path.points.empty()) return {}; - const auto shift_start_pose = road_lane_reference_path.points.back().point.pose; - - // generate Shoulder lane reference path and set it path_shifter - const auto shoulder_lane_reference_path = generateShoulderLaneReferencePath( - shoulder_lanes, shift_start_pose, goal_pose, shoulder_center_to_goal_distance); - if (shoulder_lane_reference_path.points.empty()) return {}; + shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); + const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( + road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( + road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + -before_shifted_pull_over_distance); + if (!shift_start_pose) return {}; + + // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(shoulder_lane_reference_path); - - // generate shift_line and set it to path_sifter + path_shifter.setPath(road_lane_reference_path_to_shift_end); ShiftLine shift_line{}; + shift_line.start = *shift_start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_end_road_to_target_distance; + path_shifter.addShiftLine(shift_line); + ShiftedPath shifted_path{}; + const bool offset_back = true; // offset front side from reference path + if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + + // set same orientation, because the reference center line orientation is not same to the + shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation; + + // for debug. result of shift is not equal to the target + const Pose actual_shift_end_pose = shifted_path.path.points.back().point.pose; + + // interpolate between shift end pose to goal pose + std::vector interpolated_poses = + interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); + for (const auto & pose : interpolated_poses) { + PathPointWithLaneId p = shifted_path.path.points.back(); + p.point.pose = pose; + shifted_path.path.points.push_back(p); + } + + // set goal pose with velocity 0 { - shift_line.start = shift_start_pose; - shift_line.end = shift_end_pose; - const double shoulder_left_bound_to_shift_start_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, shift_start_pose); - const double goal_to_shift_start_distance = - shoulder_left_bound_to_shift_start_distance - shoulder_left_bound_to_goal_distance; - shift_line.end_shift_length = goal_to_shift_start_distance; - path_shifter.addShiftLine(shift_line); + PathPointWithLaneId p{}; + p.point.longitudinal_velocity_mps = 0.0; + p.point.pose = goal_pose; + p.lane_ids = shifted_path.path.points.back().lane_ids; + for (const auto & lane : shoulder_lanes) { + p.lane_ids.push_back(lane.id()); + } + shifted_path.path.points.push_back(p); } - // offset front side from reference path - ShiftedPath shifted_path{}; - const bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + // set the same z as the goal + for (auto & p : shifted_path.path.points) { + p.point.pose.position.z = goal_pose.position.z; + } // check lane departure with road and shoulder lanes - lanelet::ConstLanelets lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - if (lane_departure_checker_.checkPathWillLeaveLane(lanes, shifted_path.path)) return {}; + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + if (lane_departure_checker_.checkPathWillLeaveLane( + util::transformToLanelets(expanded_lanes), shifted_path.path)) { + return {}; + } // check collision - if (!isSafePath(shifted_path.path)) return {}; + PathWithLaneId collision_check_path = shifted_path.path; + collision_check_path.points.clear(); + std::copy( + shifted_path.path.points.begin() + path_shifter.getShiftLines().front().start_idx, + shifted_path.path.points.end(), std::back_inserter(collision_check_path.points)); + + if (!isSafePath(collision_check_path)) return {}; // set lane_id and velocity to shifted_path - const auto goal_idx = motion_utils::findNearestIndex(shifted_path.path.points, goal_pose); - lanelet::ConstLanelet target_shoulder_lanelet{}; - lanelet::utils::query::getClosestLanelet( - shoulder_lanes, shifted_path.path.points.back().point.pose, &target_shoulder_lanelet); - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + for (size_t i = path_shifter.getShiftLines().front().start_idx; + i < shifted_path.path.points.size() - 1; ++i) { auto & point = shifted_path.path.points.at(i); + // set velocity + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); + + // add target lanes to points after shift start // add road lane_ids if not found - for (const auto id : road_lane_reference_path.points.back().lane_ids) { + for (const auto id : shifted_path.path.points.back().lane_ids) { if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) { point.lane_ids.push_back(id); } } // add shoulder lane_id if not found - if ( - std::find(point.lane_ids.begin(), point.lane_ids.end(), target_shoulder_lanelet.id()) == - point.lane_ids.end()) { - point.lane_ids.push_back(target_shoulder_lanelet.id()); - } - // set velocity - if (i >= *goal_idx) { - // set velocity after goal - point.point.longitudinal_velocity_mps = 0.0; - } else { - point.point.longitudinal_velocity_mps = pull_over_velocity; + for (const auto & lane : shoulder_lanes) { + if ( + std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) == + point.lane_ids.end()) { + point.lane_ids.push_back(lane.id()); + } } } + // set pull over path PullOverPath pull_over_path{}; - pull_over_path.path = - pull_over_utils::combineReferencePath(road_lane_reference_path, shifted_path.path); - // shift path is connected to one, so partial_paths have only one + pull_over_path.path = shifted_path.path; pull_over_path.partial_paths.push_back(pull_over_path.path); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; + pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(actual_shift_end_pose); // check enough distance if (!hasEnoughDistance( @@ -321,4 +299,86 @@ bool ShiftPullOver::isSafePath(const PathWithLaneId & path) const return true; } +double ShiftPullOver::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) +{ + // reverse path for checking from the end point + // note that the sign of curvature is also reversed + PathWithLaneId reversed_path{}; + std::reverse_copy( + path.points.begin(), path.points.end(), std::back_inserter(reversed_path.points)); + + double before_arc_length{0.0}; + double after_arc_length{0.0}; + for (const auto & [k, segment_length] : + motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + // after shifted segment length + const double after_segment_length = + k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + before_arc_length += k > 0 ? offset / (1 + k * dr) : offset * (1 - k * dr); + break; + } + before_arc_length += segment_length; + after_arc_length += after_segment_length; + } + + return before_arc_length; +} + +// only two points is supported +std::vector ShiftPullOver::splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s) +{ + const double h = base_s.at(1) - base_s.at(0); + + const double c = begin_diff; + const double d = base_x.at(0); + const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); + const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); + + std::vector res; + for (const auto & s : new_s) { + const double ds = s - base_s.at(0); + res.push_back(d + (c + (b + a * ds) * ds) * ds); + } + + return res; +} + +std::vector ShiftPullOver::interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval) +{ + std::vector interpolated_poses{}; // output + + const std::vector base_s{ + 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const std::vector base_x{start_pose.position.x, end_pose.position.x}; + const std::vector base_y{start_pose.position.y, end_pose.position.y}; + std::vector new_s; + + constexpr double eps = 0.3; // prevent overlapping + for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + new_s.push_back(s); + } + + const std::vector interpolated_x = splineTwoPoints( + base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), + std::cos(tf2::getYaw(end_pose.orientation)), new_s); + const std::vector interpolated_y = splineTwoPoints( + base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), + std::sin(tf2::getYaw(end_pose.orientation)), new_s); + for (size_t i = 0; i < interpolated_x.size(); ++i) { + Pose pose{}; + pose = util::lerpByPose(end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + pose.position.x = interpolated_x.at(i); + pose.position.y = interpolated_y.at(i); + pose.position.z = end_pose.position.z; + interpolated_poses.push_back(pose); + } + + return interpolated_poses; +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index addc437dfba7a..b2a7af91ef3ad 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -86,30 +86,24 @@ PredictedObjects filterObjectsByLateralDistance( return filtered_objects; } -Marker createPullOverAreaMarker( - const Pose & start_pose, const Pose & end_pose, const int32_t id, - const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, - const double vehicle_width, const std_msgs::msg::ColorRGBA & color) +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z) { - Marker marker = createDefaultMarker( - header.frame_id, header.stamp, "pull_over_area", id, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); - - auto p_left_front = calcOffsetPose(end_pose, base_link2front, vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); - - auto p_right_front = calcOffsetPose(end_pose, base_link2front, -vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_right_front.x, p_right_front.y, p_right_front.z)); - - auto p_right_back = calcOffsetPose(start_pose, -base_link2rear, -vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_right_back.x, p_right_back.y, p_right_back.z)); - - auto p_left_back = calcOffsetPose(start_pose, -base_link2rear, vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_left_back.x, p_left_back.y, p_left_back.z)); + MarkerArray marker_array{}; + for (size_t i = 0; i < area_polygons.size(); ++i) { + Marker marker = createDefaultMarker( + header.frame_id, header.stamp, "pull_over_area_" + std::to_string(i), i, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); + const auto & poly = area_polygons.at(i); + for (const auto & p : poly.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), z)); + } - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); + marker_array.markers.push_back(marker); + } - return marker; + return marker_array; } MarkerArray createPosesMarkerArray( @@ -149,7 +143,7 @@ MarkerArray createTextsMarkerArray( } MarkerArray createGoalCandidatesMarkerArray( - std::vector & goal_candidates, const std_msgs::msg::ColorRGBA & color) + GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) { // convert to pose vector std::vector pose_vector{}; diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 4ce7c253c51be..9b4ba77a3c7f6 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -163,6 +163,16 @@ std::vector GeometricParallelParking::generatePullOverPaths( // straight path from current to parking start const auto straight_path = generateStraightPath(start_pose); + // check the continuity of straight path and arc path + const Pose & road_path_last_pose = straight_path.points.back().point.pose; + const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; + const double yaw_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_last_pose.orientation), tf2::getYaw(arc_path_first_pose.orientation)); + const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); + if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + return std::vector{}; + } + // combine straight_path -> arc_path*2 auto paths = arc_paths; paths.insert(paths.begin(), straight_path); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index c38b54086b3fd..b899439215a02 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1526,6 +1526,61 @@ double getSignedDistanceFromShoulderLeftBoundary( return arc_coordinates.distance; } +std::optional getSignedDistanceFromShoulderLeftBoundary( + const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, + const Pose & vehicle_pose) +{ + double min_distance = std::numeric_limits::max(); + const auto transformed_footprint = + transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); + bool found_neighbor_shoulder_bound = false; + for (const auto & vehicle_corner_point : transformed_footprint) { + // convert point of footprint to pose + Pose vehicle_corner_pose{}; + vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.orientation = vehicle_pose.orientation; + + // calculate distance to the shoulder bound directly next to footprint points + lanelet::ConstLanelet closest_shoulder_lanelet{}; + if (lanelet::utils::query::getClosestLanelet( + shoulder_lanelets, vehicle_corner_pose, &closest_shoulder_lanelet)) { + const auto & left_line_2d = lanelet::utils::to2D(closest_shoulder_lanelet.leftBound3d()); + + for (size_t i = 1; i < left_line_2d.size(); ++i) { + const Point p_front = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i - 1]); + const Point p_back = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i]); + + const Point inverse_p_front = + tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); + const Point inverse_p_back = + tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); + + const double dy_front = inverse_p_front.y; + const double dy_back = inverse_p_back.y; + const double dx_front = inverse_p_front.x; + const double dx_back = inverse_p_back.x; + // is in segment + if (dx_front < 0 && dx_back > 0) { + const double lateral_distance_from_pose_to_segment = + (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); + min_distance = std::min(lateral_distance_from_pose_to_segment, min_distance); + found_neighbor_shoulder_bound = true; + break; + } + } + } + } + + if (!found_neighbor_shoulder_bound) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utilities"), + "neighbor shoulder bound to footprint is not found."); + return {}; + } + + return -min_distance; +} + double getSignedDistanceFromRightBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose) { diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index e3e4c6d75d6ed..a70ab56d98a94 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -87,7 +87,7 @@ class TrajectoryAnalyzer TrajectoryDebugInfo data; data.stamp = node_->now(); data.size = points.size(); - data.curvature = calcCurvature(points); + data.curvature = motion_utils::calcCurvature(points); const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p); data.arclength = calcPathArcLengthArray(points, -arclength_offset); data.velocity = getVelocityArray(points); diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index e3b4ab7639cb6..96488fbde4b96 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -114,21 +114,6 @@ std::vector calcPathArcLengthArray(const T & points, const double offset return out; } -template -inline std::vector calcCurvature(const T & points) -{ - std::vector curvature_arr; - curvature_arr.push_back(0.0); - for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = getPoint(points.at(i - 1)); - const auto p2 = getPoint(points.at(i)); - const auto p3 = getPoint(points.at(i + 1)); - curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); - } - curvature_arr.push_back(0.0); - return curvature_arr; -} - } // namespace planning_debug_tools #endif // PLANNING_DEBUG_TOOLS__UTIL_HPP_ From 7ef07098ea95aa9ba0c5f245f25e46eed72fb5e2 Mon Sep 17 00:00:00 2001 From: Yusuke Muramatsu Date: Thu, 22 Dec 2022 15:59:40 +0900 Subject: [PATCH 04/11] feat(tier4_perception_launch): change the merge priority of roi_cluster_fusion to the lowest (#2522) Signed-off-by: yukke42 Signed-off-by: yukke42 --- .../detection/camera_lidar_fusion_based_detection.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 74d4cbe98de44..9dca7a7767321 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -246,6 +246,7 @@ + From 9b9c90bf239714747c704a5a5de37faa7d073b14 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 22 Dec 2022 16:00:08 +0900 Subject: [PATCH 05/11] fix(tier4_planning_launch): remove unintended config file (#2554) Signed-off-by: kminoda Signed-off-by: kminoda --- .../intersection.param.yaml | 26 ------------------- 1 file changed, 26 deletions(-) delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml deleted file mode 100644 index 220c087d18de0..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ /dev/null @@ -1,26 +0,0 @@ -/**: - ros__parameters: - intersection: - state_transit_margin_time: 0.5 - stop_line_margin: 3.0 - keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr - keep_detection_vel_thr: 0.833 # == 3.0km/h - stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) - stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h - intersection_velocity: 2.778 # 2.778m/s = 10.0km/h - intersection_max_accel: 0.5 # m/ss - detection_area_margin: 0.5 # [m] - detection_area_right_margin: 0.5 # [m] - detection_area_left_margin: 0.5 # [m] - detection_area_length: 200.0 # [m] - detection_area_angle_threshold: 0.785 # [rad] - min_predicted_path_confidence: 0.05 - collision_start_margin_time: 5.0 # [s] - collision_end_margin_time: 2.0 # [s] - use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck - assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning - enable_front_car_decel_prediction: false # By default this feature is disabled - - merge_from_private_road: - stop_duration_sec: 1.0 From 6feb52ad5c8a69f97fad17c51f3c676c8c45d42d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Dec 2022 16:47:26 +0900 Subject: [PATCH 06/11] chore(motion_utils): add code owners (#2564) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- common/motion_utils/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index d8f6340a473e4..ee6975e0caa83 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -8,7 +8,11 @@ Satoshi Ota Takayuki Murooka + Fumiya Watanabe + Kosuke Takeuchi Taiki Tanaka + Takamasa Horibe + Tomoya Kimura Apache License 2.0 Yutaka Shimizu From 4600cf24f8c28411d664134cf60fca083c3e975d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Dec 2022 16:58:23 +0900 Subject: [PATCH 07/11] feat(behavior_path_planner): pull over no stopping area (#2492) * feat(behavior_path_planner): pull over no stopping area Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp Signed-off-by: kosuke55 --- .../scene_module/pull_over/goal_searcher.hpp | 3 ++ .../scene_module/pull_over/goal_searcher.cpp | 31 +++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp index f26d0e4cdebfe..de79f725e01c7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp @@ -27,6 +27,7 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::LinearRing2d; +using BasicPolygons2d = std::vector; class GoalSearcher : public GoalSearcherBase { @@ -42,6 +43,8 @@ class GoalSearcher : public GoalSearcherBase bool checkCollision(const Pose & pose) const; bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; + BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; + bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; LinearRing2d vehicle_footprint_{}; std::shared_ptr occupancy_grid_map_{}; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index dea04a5ab85fe..c75d216730e50 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -26,6 +26,7 @@ namespace behavior_path_planner { using lane_departure_checker::LaneDepartureChecker; +using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; @@ -86,9 +87,15 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); + + if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { + continue; + } + if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { continue; } + if (checkCollision(search_pose)) { continue; } @@ -241,4 +248,28 @@ void GoalSearcher::createAreaPolygons(std::vector original_search_poses) } } +BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const +{ + BasicPolygons2d area_polygons{}; + for (const auto & ll : lanes) { + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + for (const auto & area : reg_elem->noStoppingAreas()) { + const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); + area_polygons.push_back(area_poly); + } + } + } + return area_polygons; +} + +bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const +{ + for (const auto & area : areas) { + if (boost::geometry::intersects(area, footprint)) { + return true; + } + } + return false; +} + } // namespace behavior_path_planner From bfb55a3cb3ec76f2d0a6f01b725d30c47eaaacd3 Mon Sep 17 00:00:00 2001 From: yabuta Date: Thu, 22 Dec 2022 17:12:20 +0900 Subject: [PATCH 08/11] fix(accel brake map calibrator): correction of forgotten support for util files change (#2509) * fix accel brake map image server for api * get calibration_method default value from parameter * ci(pre-commit): autofix * fix spell Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../accel_brake_map_calibrator.launch.xml | 5 ++- .../scripts/new_accel_brake_map_server.py | 38 +++++++++++++++++-- 2 files changed, 38 insertions(+), 5 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml index c3379ee67f7ca..69a65a8d540a2 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml @@ -8,6 +8,7 @@ + @@ -25,7 +26,9 @@ - + + + diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index ca65b332093a7..9b5cfc9186495 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -35,15 +35,43 @@ class DrawGraph(Node): def __init__(self): super().__init__("plot_server") + self.srv = self.create_service( CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback ) - package_path = get_package_share_directory("accel_brake_map_calibrator") default_map_path = get_package_share_directory("raw_vehicle_cmd_converter") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_default_map_dir", default_map_path + "/data/default/" + ) + self.default_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") + .get_parameter_value() + .string_value + ) + + package_path = get_package_share_directory("accel_brake_map_calibrator") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" + ) + self.calibrated_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") + .get_parameter_value() + .string_value + ) + + self.declare_parameter("calibration_method", "each_cell") + self.calibration_method = ( + self.get_parameter("calibration_method").get_parameter_value().string_value + ) + if self.calibration_method is None: + self.calibration_method = "each_cell" + elif not ( + (self.calibration_method == "each_cell") | (self.calibration_method == "four_cell") + ): + print("invalid method.") + self.calibration_method = "each_cell" - self.default_map_dir = default_map_path + "/data/default/" - self.calibrated_map_dir = package_path + "/config/" self.log_file = package_path + "/config/log.csv" config_file = package_path + "/config/accel_brake_map_calibrator.param.yaml" @@ -71,6 +99,7 @@ def __init__(self): # debug self.get_logger().info("default map dir: {}".format(self.default_map_dir)) self.get_logger().info("calibrated map dir: {}".format(self.calibrated_map_dir)) + self.get_logger().info("calibrated method: {}".format(self.calibration_method)) self.get_logger().info("log file :{}".format(self.log_file)) self.get_logger().info("min_vel_thr : {}".format(self.min_vel_thr)) self.get_logger().info("vel_diff_thr : {}".format(self.vel_diff_thr)) @@ -82,7 +111,7 @@ def __init__(self): def get_data_callback(self, request, response): # read csv - # If log file doesn't exsist, return empty data + # If log file doesn't exist, return empty data if not Path(self.log_file).exists(): response.graph_image = [] self.get_logger().info("svg data is empty") @@ -123,6 +152,7 @@ def get_data_callback(self, request, response): self.vel_diff_thr, CF.PEDAL_LIST, self.pedal_diff_thr, + self.calibration_method, ) count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) From 98b1cf3a18cdf3c02ae6a0c6e431a706448a026c Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 22 Dec 2022 17:46:20 +0900 Subject: [PATCH 09/11] chore(lidar_centerpoint): updated centerpoint models and documentation (#2553) * Added documentation for the new models Signed-off-by: Kenzo Lobos-Tsunekawa * Changed the model links' in the CMakeList Signed-off-by: Kenzo Lobos-Tsunekawa * Update perception/lidar_centerpoint/README.md Co-authored-by: Yusuke Muramatsu * Added the raw links in the README Signed-off-by: Kenzo Lobos-Tsunekawa Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: Yusuke Muramatsu --- perception/lidar_centerpoint/CMakeLists.txt | 8 ++++---- perception/lidar_centerpoint/README.md | 10 ++++++++++ 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 2591266652827..a3938423d6612 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -107,12 +107,12 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) endfunction() # centerpoint - download(v1 pts_voxel_encoder_centerpoint.onnx 94dcf01ccd53ef14a79787afaada63b4) - download(v1 pts_backbone_neck_head_centerpoint.onnx 7046995e502e11f35c9459668d8afde3) + download(v2 pts_voxel_encoder_centerpoint.onnx e8369d7e0f59951cf87c0274830de1fe) + download(v2 pts_backbone_neck_head_centerpoint.onnx 44e44d26781a69c741bf9bddde57fbb3) # centerpoint_tiny - download(v1 pts_voxel_encoder_centerpoint_tiny.onnx 410f730c537968cb27fbd70c941849a8) - download(v1 pts_backbone_neck_head_centerpoint_tiny.onnx e97c165c7877222c0e27e44409a07517) + download(v2 pts_voxel_encoder_centerpoint_tiny.onnx 5fb774fac44dc83949d05eb3ba077309) + download(v2 pts_backbone_neck_head_centerpoint_tiny.onnx e4658325b70222f7c3637fe00e586b82) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 777b3fc191111..baaea06e59346 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -52,6 +52,12 @@ We trained the models using . You can download the onnx format of trained models by clicking on the links below. +- Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx) +- Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx) + +`Centerpoint` was trained in `nuScenes` (~110k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. +`Centerpoint tiny` was trained in `Argoverse 2` (~28k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. + ## Standalone inference and visualization In addition to its use as a standard ROS node, `lidar_centerpoint` can also be used to perform inferences in an isolated manner. @@ -119,6 +125,10 @@ Example: [7] +[8] + +[9] + ## (Optional) Future extensions / Unimplemented parts