From 34949d8c6fec70be8c8441803959670c9f6a288d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 10 Oct 2023 18:01:13 +0900 Subject: [PATCH 01/14] fix(lane_change): add guard to neigibor lanelets (#5245) Signed-off-by: kosuke55 --- .../src/scene_module/lane_change/normal.cpp | 3 +++ planning/behavior_path_planner/src/utils/lane_change/utils.cpp | 3 +++ 2 files changed, 6 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c8dbba21c7094..3a7dd85a4d41d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -942,6 +942,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + return false; + } const auto target_objects = getTargetObjects(current_lanes, target_lanes); debug_filtered_objects_ = target_objects; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index a248d8aeb2063..8337cbee58583 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -242,6 +242,9 @@ lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( { const auto target_neighbor_lanelets = utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); + if (target_neighbor_lanelets.empty()) { + return {}; + } const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( target_neighbor_lanelets, 0, std::numeric_limits::max()); From 41690c52f086525bf50094d522b1d9ad5a72675c Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 12 Oct 2023 12:18:48 +0900 Subject: [PATCH 02/14] feat(lane_change): sampling all possible longitudinal acceleration when the ego is in stuck (#5249) * [lane_change] sampling all possible longitudinal acceleration when the ego is in stuck Signed-off-by: Takamasa Horibe * add clock Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Fumiya Watanabe * fix style Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Fumiya Watanabe --- .../scene_module/lane_change/normal.hpp | 13 ++ .../src/scene_module/lane_change/normal.cpp | 120 ++++++++++++++---- 2 files changed, 108 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index f1e4483c8bf51..4f1eb7340fdff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -153,6 +153,19 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. + //! @param obstacle_check_distance Distance to check ahead for any objects that might be + //! obstructing ego path. It makes sense to use values like the maximum lane change distance. + bool isVehicleStuckByObstacle( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; + + bool isVehicleStuckByObstacle(const lanelet::ConstLanelets & current_lanes) const; + + double calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; + + std::pair calcCurrentMinMaxAcceleration() const; + void setStopPose(const Pose & stop_pose); void updateStopTime(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 3a7dd85a4d41d..d12b4a0eec717 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -504,6 +504,37 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } +std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() const +{ + const auto & p = getCommonParam(); + + const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); + + const auto ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_path_.points, getEgoPose(), p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + const auto max_path_velocity = + prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps; + + // calculate minimum and maximum acceleration + const auto min_acc = + utils::lane_change::calcMinimumAcceleration(getEgoVelocity(), vehicle_min_acc, p); + const auto max_acc = utils::lane_change::calcMaximumAcceleration( + getEgoVelocity(), max_path_velocity, vehicle_max_acc, p); + + return {min_acc, max_acc}; +} + +double NormalLaneChange::calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const +{ + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); + return utils::lane_change::calcMaximumLaneChangeLength( + getEgoVelocity(), getCommonParam(), shift_intervals, max_acc); +} + std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { @@ -511,29 +542,11 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return {}; } - const auto & common_parameters = planner_data_->parameters; const auto & route_handler = *getRouteHandler(); const auto current_pose = getEgoPose(); - const auto current_velocity = getEgoVelocity(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - const auto vehicle_min_acc = - std::max(common_parameters.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto vehicle_max_acc = - std::min(common_parameters.max_acc, lane_change_parameters_->max_longitudinal_acc); - const double nearest_dist_threshold = common_parameters.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_parameters.ego_nearest_yaw_threshold; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_module_path_.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double & max_path_velocity = - prev_module_path_.points.at(current_seg_idx).point.longitudinal_velocity_mps; - - // calculate minimum and maximum acceleration - const auto min_acc = utils::lane_change::calcMinimumAcceleration( - current_velocity, vehicle_min_acc, common_parameters); - const auto max_acc = utils::lane_change::calcMaximumAcceleration( - current_velocity, max_path_velocity, vehicle_max_acc, common_parameters); + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { @@ -542,15 +555,22 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // calculate maximum lane change length - const double max_lane_change_length = utils::lane_change::calcMaximumLaneChangeLength( - current_velocity, common_parameters, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } + // If the ego is in stuck, sampling all possible accelerations to find avoiding path. + if (isVehicleStuckByObstacle(current_lanes, max_lane_change_length)) { + auto clock = rclcpp::Clock(RCL_ROS_TIME); + RCLCPP_INFO_THROTTLE( + logger_, clock, 1000, "Vehicle is stuck. sample all longitudinal acceleration."); + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + // if maximum lane change length is less than length to goal or the end of target lanes, only // sample max acc if (route_handler.isInGoalRouteSection(target_lanes.back())) { @@ -1118,8 +1138,9 @@ bool NormalLaneChange::getLaneChangePaths( if (getStopTime() < lane_change_parameters_->stop_time_threshold) { continue; } - RCLCPP_WARN_STREAM( - logger_, "Stop time is over threshold. Allow lane change in crosswalk."); + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_THROTTLE( + logger_, clock, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); } if ( @@ -1448,8 +1469,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::lane_change::convertToPredictedPath(ego_predicted_path, time_resolution); auto collision_check_objects = target_objects.target_lane; + const auto current_lanes = getCurrentLanes(); + const auto is_stuck = isVehicleStuckByObstacle(current_lanes); - if (lane_change_parameters_->check_objects_on_current_lanes) { + if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) { collision_check_objects.insert( collision_check_objects.end(), target_objects.current_lane.begin(), target_objects.current_lane.end()); @@ -1504,6 +1527,53 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return path_safety_status; } +// Check if the ego vehicle is in stuck by a stationary obstacle. +bool NormalLaneChange::isVehicleStuckByObstacle( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const +{ + // Ego is still moving, not in stuck + if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + using lanelet::utils::getArcCoordinates; + const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; + + for (const auto & object : debug_filtered_objects_.current_lane) { + const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + + // Note: it needs chattering prevention. + if (std::abs(object.initial_twist.twist.linear.x) > 0.3) { // check if stationary + continue; + } + + const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; + if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { + return true; // Stationary object is in front of ego. + } + } + + // No stationary objects found in obstacle_check_distance + return false; +} + +bool NormalLaneChange::isVehicleStuckByObstacle(const lanelet::ConstLanelets & current_lanes) const +{ + if (current_lanes.empty()) { + return false; // can not check + } + + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); + const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); + return isVehicleStuckByObstacle(current_lanes, max_lane_change_length); +} + void NormalLaneChange::setStopPose(const Pose & stop_pose) { lane_change_stop_pose_ = stop_pose; From 15b140210053beee3b3f38b427788e488dc5b9ca Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 12 Oct 2023 11:49:11 +0900 Subject: [PATCH 03/14] feat(lane_change): keep distance against avoidable stationary objects (#5236) * feat(lane_change): keep distance against avoidable stationary objects Signed-off-by: kosuke55 consider rss distance Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 * fix dangling Signed-off-by: Takamasa Horibe * use parameter for velocity_treshold Signed-off-by: Takamasa Horibe * check object only on the ego path Signed-off-by: Takamasa Horibe * fix Signed-off-by: Takamasa Horibe --------- Signed-off-by: kosuke55 Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe --- .../src/scene_module/lane_change/normal.cpp | 83 ++++++++++++++++--- 1 file changed, 73 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d12b4a0eec717..940da34083a44 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -33,6 +33,8 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; + NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, Direction direction) @@ -135,7 +137,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() if (isStopState()) { const auto current_velocity = getEgoVelocity(); - const auto current_dist = motion_utils::calcSignedArcLength( + const auto current_dist = calcSignedArcLength( output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); @@ -198,7 +200,70 @@ void NormalLaneChange::insertStopPoint( } const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; - const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); + const bool is_in_terminal_section = lanelet::utils::isInLanelet(getEgoPose(), lanelets.back()) || + distance_to_terminal < lane_change_buffer; + const bool has_blocking_target_lane_obj = std::any_of( + target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + if (o.initial_twist.twist.linear.x > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + const double distance_to_target_lane_obj = utils::getSignedDistance( + path.points.front().point.pose, o.initial_pose.pose, status_.target_lanes); + return distance_to_target_lane_obj < distance_to_terminal; + }); + + // auto stopping_distance = raw_stopping_distance; + double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + if (is_in_terminal_section || !has_blocking_target_lane_obj) { + // calculate minimum distance from path front to the stationary object on the ego lane. + const auto distance_to_ego_lane_obj = [&]() -> double { + double distance_to_obj = distance_to_terminal; + const double distance_to_ego = + utils::getSignedDistance(path.points.front().point.pose, getEgoPose(), lanelets); + + for (const auto & object : target_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : polygon) { + const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + + // ignore if the point is around the ego path + if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { + continue; + } + + const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + + // ignore backward object + if (current_distance_to_obj < distance_to_ego) { + continue; + } + distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); + } + } + return distance_to_obj; + }(); + + // If the lane change space is occupied by a stationary obstacle, move the stop line closer. + // TODO(Horibe): We need to loop this process because the new space could be occupied as well. + stopping_distance = std::min( + distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer - + getCommonParam().base_link2front - + calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params), + stopping_distance); + } + if (stopping_distance > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); setStopPose(stop_point.point.pose); @@ -216,8 +281,7 @@ std::optional NormalLaneChange::extendPath() const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; - const auto dist = - motion_utils::calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); + const auto dist = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); if (dist < 0.0) { return std::nullopt; @@ -419,7 +483,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -435,7 +499,7 @@ bool NormalLaneChange::isEgoOnPreparePhase() const { const auto & start_position = status_.lane_change_path.info.shift_line.start.position; const auto & path_points = status_.lane_change_path.path.points; - return motion_utils::calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; + return calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; } bool NormalLaneChange::isAbleToStopSafely() const @@ -455,7 +519,7 @@ bool NormalLaneChange::isAbleToStopSafely() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -471,7 +535,7 @@ bool NormalLaneChange::hasFinishedAbort() const return true; } - const auto distance_to_finish = motion_utils::calcSignedArcLength( + const auto distance_to_finish = calcSignedArcLength( abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position); if (distance_to_finish < 0.0) { @@ -736,8 +800,7 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( double min_dist_ego_to_obj = std::numeric_limits::max(); for (const auto & polygon_p : obj_polygon.outer()) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } From 86f68a972c3e166b001e89862955851d1aa1f340 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 12 Oct 2023 17:22:10 +0900 Subject: [PATCH 04/14] fix(lane_change): fixed debug visualization bug (#5284) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 940da34083a44..46dc5839593d6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1556,12 +1556,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); + auto is_safe = true; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second); if (collided_polygons.empty()) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } @@ -1571,20 +1573,20 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } + is_safe = false; path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); } - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); } return path_safety_status; From c4bc4b4af230fe853121e0b852c74952d2563aa9 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 12 Oct 2023 19:09:30 +0900 Subject: [PATCH 05/14] fix(lane_change): update target lane if valid path not found (#5287) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 46dc5839593d6..c775236dfbc6a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -85,6 +85,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; + safe_path.info.target_lanes = target_lanes; return {false, false}; } From e39ea7f1500d50e06e1a38925067ed61dcef53d9 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 13 Oct 2023 15:39:40 +0900 Subject: [PATCH 06/14] feat(lane_change): add rss paramas for stuck Signed-off-by: kosuke55 --- .../config/lane_change/lane_change.param.yaml | 8 +++++ .../scene_module/lane_change/base_class.hpp | 3 +- .../scene_module/lane_change/normal.hpp | 3 +- .../lane_change/lane_change_module_data.hpp | 1 + .../src/scene_module/lane_change/manager.cpp | 15 +++++++++ .../src/scene_module/lane_change/normal.cpp | 33 +++++++++++++------ 6 files changed, 51 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index e7f3b51bd2d26..d3c0a22e867f9 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -45,6 +45,14 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.6 + stuck: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # lane expansion for object filtering lane_expansion: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index a3469dee2909b..26f68e69b3728 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -230,7 +230,8 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths, const bool check_safety) const = 0; + LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, + const bool is_stuck, const bool check_safety) const = 0; virtual TurnSignalInfo calcTurnSignalInfo() = 0; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 4f1eb7340fdff..e9e361e1c39b8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -138,6 +138,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; TurnSignalInfo calcTurnSignalInfo() override; @@ -146,7 +147,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, CollisionCheckDebugMap & debug_data) const; LaneChangeTargetObjectIndices filterObject( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index e74a6d7b72b66..d37b71989f4e9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -83,6 +83,7 @@ struct LaneChangeParameters bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; + utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort LaneChangeCancelParameters cancel; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 8b865b37b6969..a9d77fef2f2f5 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -125,6 +125,21 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); + p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold")); + p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time")); + p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_front_deceleration")); + p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_rear_deceleration")); + p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_reaction_time")); + p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin")); + p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c775236dfbc6a..7cc381d2d9e12 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -79,8 +79,17 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) // find candidate paths LaneChangePaths valid_paths{}; - const auto found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); + const bool is_stuck = isVehicleStuckByObstacle(current_lanes); + bool found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, lane_change_parameters_->rss_params, + is_stuck); + // if no safe path is found and ego is stuck, try to find a path with a small margin + if (!found_safe_path && is_stuck) { + found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, + lane_change_parameters_->rss_params_for_stuck, is_stuck); + } + debug_valid_path_ = valid_paths; if (valid_paths.empty()) { @@ -988,7 +997,9 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const + Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, + const bool check_safety) const { object_debug_.clear(); if (current_lanes.empty() || target_lanes.empty()) { @@ -1219,9 +1230,11 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); - if (utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + if ( + !is_stuck && + utils::lane_change::passParkedObject( + route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_, object_debug_)) { return false; } @@ -1230,7 +1243,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); + *candidate_path, target_objects, rss_params, is_stuck, object_debug_); if (is_safe) { return true; @@ -1252,8 +1265,9 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const debug_filtered_objects_ = target_objects; CollisionCheckDebugMap debug_data; + const bool is_stuck = isVehicleStuckByObstacle(current_lanes); const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1510,7 +1524,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, CollisionCheckDebugMap & debug_data) const { PathSafetyStatus path_safety_status; @@ -1534,7 +1548,6 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto collision_check_objects = target_objects.target_lane; const auto current_lanes = getCurrentLanes(); - const auto is_stuck = isVehicleStuckByObstacle(current_lanes); if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) { collision_check_objects.insert( From d5e9b951c8bec9f7874ec45ab2cc4bd55c3cf464 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 13 Oct 2023 16:41:16 +0900 Subject: [PATCH 07/14] fix(lane_change): fix chattering for stopped objects Signed-off-by: kosuke55 Update planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp fix(path_safety_checker): remove redundant parameter name Signed-off-by: Fumiya Watanabe --- .../path_safety_checker_parameters.hpp | 25 ++++---- .../path_safety_checker/safety_check.hpp | 5 +- .../src/marker_utils/utils.cpp | 5 +- .../path_safety_checker/safety_check.cpp | 60 ++++++++++++------- .../test/test_safety_check.cpp | 19 +++--- 5 files changed, 69 insertions(+), 45 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index d4b43ed4c0613..d027640e55bd0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -179,18 +179,19 @@ struct SafetyCheckParams struct CollisionCheckDebug { - std::string unsafe_reason; ///< Reason indicating unsafe situation. - Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. - Pose current_obj_pose{}; ///< Detected object's current pose. - Twist object_twist{}; ///< Detected object's velocity and rotation. - Pose expected_obj_pose{}; ///< Predicted future pose of object. - double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. - double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. - double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. - double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. - bool is_front{false}; ///< True if object is in front of ego vehicle. - bool is_safe{false}; ///< True if situation is deemed safe. + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double forward_lon_offset{0.0}; ///< Forward longitudinal offset for extended polygon. + double backward_lon_offset{0.0}; ///< Backward longitudinal offset for extended polygon. + double lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. std::vector ego_predicted_path; ///< ego vehicle's predicted path. std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index c9cedca39aca0..8d8abccbd5413 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -61,10 +61,11 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug); + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug); + const double is_stopped_obj, CollisionCheckDebug & debug); PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution); diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 7dc85de879a18..06c0111786ef9 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -636,8 +636,9 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal << "\nEgo to obj: " << info.inter_vehicle_distance << "\nExtended polygon: " << (info.is_front ? "ego" : "object") - << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset - << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nExtended polygon lateral offset: " << info.lat_offset + << "\nExtended polygon forward longitudinal offset: " << info.forward_lon_offset + << "\nExtended polygon backward longitudinal offset: " << info.backward_lon_offset << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") << "\nSafe: " << (info.is_safe ? "Yes" : "No"); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 6ea21faef2e2f..778d999b011b0 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -70,28 +70,33 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug) + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; const double & width = vehicle_info.vehicle_width_m; const double & base_to_rear = vehicle_info.rear_overhang_m; - const double lon_offset = std::max(lon_length + base_to_front, base_to_front); - + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = + -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value const double lat_offset = width / 2.0 + lat_margin; { - debug.extended_polygon_lon_offset = lon_offset; - debug.extended_polygon_lat_offset = lat_offset; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = lat_offset; } - const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -106,7 +111,7 @@ Polygon2d createExtendedPolygon( Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug) + const double is_stopped_obj, CollisionCheckDebug & debug) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { @@ -127,19 +132,27 @@ Polygon2d createExtendedPolygon( min_y = std::min(transformed_p.y, min_y); } - const double lon_offset = max_x + lon_length; + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = max_x + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = min_x - (is_stopped_obj ? lon_length / 2 : 0); // minus value + const double left_lat_offset = max_y + lat_margin; const double right_lat_offset = min_y - lat_margin; { - debug.extended_polygon_lon_offset = lon_offset; - debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = (left_lat_offset + right_lat_offset) / 2; } - const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); - const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0); - const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0); - const auto p4 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, left_lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + const auto p2 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + const auto p3 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + const auto p4 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -326,14 +339,17 @@ std::vector getCollidedPolygons( const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; - const auto & extended_ego_polygon = - is_object_front - ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) - : ego_polygon; + // TODO(watanabe) fix hard coding value + const bool is_stopped_object = object_velocity < 0.3; + const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, + lat_margin, is_stopped_object, debug) + : ego_polygon; const auto & extended_obj_polygon = is_object_front ? obj_polygon - : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin, debug); + : createExtendedPolygon( + obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); { debug.rss_longitudinal = rss_dist; diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 9dfcef487cc81..d60957c7d41b4 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -51,9 +51,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -78,9 +79,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -106,9 +108,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -154,9 +157,11 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; CollisionCheckDebug debug; - const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, debug); + const auto polygon = + createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); From 449cb67321129accc2e327e16b2521e5e9278cb9 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 13 Oct 2023 19:38:17 +0900 Subject: [PATCH 08/14] fix(lane_change): add margin in stuck detection distance Signed-off-by: Takamasa Horibe use margin Signed-off-by: Takamasa Horibe --- .../src/scene_module/lane_change/normal.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 7cc381d2d9e12..88dbe0dabfbd6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -637,7 +637,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuckByObstacle(current_lanes, max_lane_change_length)) { + if (isVehicleStuckByObstacle(current_lanes)) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is stuck. sample all longitudinal acceleration."); @@ -1650,7 +1650,21 @@ bool NormalLaneChange::isVehicleStuckByObstacle(const lanelet::ConstLanelets & c const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); - return isVehicleStuckByObstacle(current_lanes, max_lane_change_length); + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + // It is difficult to define the detection range. If it is too short, the stuck will not be + // determined, even though you are stuck by an obstacle. If it is too long, + // the ego will be judged to be stuck by a distant vehicle, even though the ego is only + // stopped at a traffic light. Essentially, the calculation should be based on the information of + // the stop reason, but this is outside the scope of one module. I keep it as a TODO. + constexpr double DETECTION_DISTANCE_MARGIN = 10.0; + const auto detection_distance = max_lane_change_length + rss_dist + + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; + RCLCPP_INFO(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + + return isVehicleStuckByObstacle(current_lanes, detection_distance); } void NormalLaneChange::setStopPose(const Pose & stop_pose) From 63993a766a63b971730d91950b28a99c37e51429 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 13 Oct 2023 19:28:50 +0900 Subject: [PATCH 09/14] fix(lane_change): update target lane obj block condition (#5296) fix(lane_change): target lane blocking condition remove in_terminal condition fix Signed-off-by: Takamasa Horibe --- .../src/scene_module/lane_change/normal.cpp | 122 ++++++++++-------- 1 file changed, 68 insertions(+), 54 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 88dbe0dabfbd6..5c149cf0695ba 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -199,79 +199,93 @@ void NormalLaneChange::insertStopPoint( const double lane_change_buffer = utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { + return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); + }; + // If lanelets.back() is in goal route section, get distance to goal. // Otherwise, get distance to end of lane. double distance_to_terminal = 0.0; if (route_handler->isInGoalRouteSection(lanelets.back())) { const auto goal = route_handler->getGoalPose(); - distance_to_terminal = utils::getSignedDistance(path.points.front().point.pose, goal, lanelets); + distance_to_terminal = getDistanceAlongLanelet(goal); } else { distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); } const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); - const bool is_in_terminal_section = lanelet::utils::isInLanelet(getEgoPose(), lanelets.back()) || - distance_to_terminal < lane_change_buffer; - const bool has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { - if (o.initial_twist.twist.linear.x > lane_change_parameters_->stop_velocity_threshold) { - return false; + double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + + // calculate minimum distance from path front to the stationary object on the ego lane. + const auto distance_to_ego_lane_obj = [&]() -> double { + double distance_to_obj = distance_to_terminal; + const double distance_to_ego = getDistanceAlongLanelet(getEgoPose()); + + for (const auto & object : target_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + continue; } - const double distance_to_target_lane_obj = utils::getSignedDistance( - path.points.front().point.pose, o.initial_pose.pose, status_.target_lanes); - return distance_to_target_lane_obj < distance_to_terminal; - }); - // auto stopping_distance = raw_stopping_distance; - double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; - if (is_in_terminal_section || !has_blocking_target_lane_obj) { - // calculate minimum distance from path front to the stationary object on the ego lane. - const auto distance_to_ego_lane_obj = [&]() -> double { - double distance_to_obj = distance_to_terminal; - const double distance_to_ego = - utils::getSignedDistance(path.points.front().point.pose, getEgoPose(), lanelets); - - for (const auto & object : target_objects.current_lane) { - // check if stationary - const auto obj_v = std::abs(object.initial_twist.twist.linear.x); - if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : polygon) { + const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + + // ignore if the point is around the ego path + if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { continue; } - // calculate distance from path front to the stationary object polygon on the ego lane. - const auto polygon = - tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); - for (const auto & polygon_p : polygon) { - const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } - - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); - - // ignore backward object - if (current_distance_to_obj < distance_to_ego) { - continue; - } - distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); + // ignore backward object + if (current_distance_to_obj < distance_to_ego) { + continue; } + distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); } - return distance_to_obj; - }(); - - // If the lane change space is occupied by a stationary obstacle, move the stop line closer. - // TODO(Horibe): We need to loop this process because the new space could be occupied as well. - stopping_distance = std::min( - distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer - - getCommonParam().base_link2front - - calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, - lane_change_parameters_->rss_params), - stopping_distance); + } + return distance_to_obj; + }(); + + // Need to stop before blocking obstacle + if (distance_to_ego_lane_obj < distance_to_terminal) { + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer - + stop_point_buffer - rss_dist - + getCommonParam().base_link2front; + + // If the target lane in the lane change section is blocked by a stationary obstacle, there + // is no reason for stopping with a lane change margin. Instead, stop right behind the + // obstacle. + // ---------------------------------------------------------- + // [obj]> + // ---------------------------------------------------------- + // [ego]> | <--- lane change margin ---> [obj]> + // ---------------------------------------------------------- + const bool has_blocking_target_lane_obj = std::any_of( + target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + const auto v = std::abs(o.initial_twist.twist.linear.x); + if (v > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + return stopping_distance_for_obj < distance_to_target_lane_obj && + distance_to_target_lane_obj < distance_to_ego_lane_obj; + }); + + if (!has_blocking_target_lane_obj) { + stopping_distance = stopping_distance_for_obj; + } } if (stopping_distance > 0.0) { From 951af6c6276b00f8db70407cb0c8f982a47a5936 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 13 Oct 2023 20:45:02 +0900 Subject: [PATCH 10/14] refactor(lane_change): add debug log Signed-off-by: Takamasa Horibe update Signed-off-by: Takamasa Horibe --- .../config/logger_config.yaml | 4 + .../scene_module/lane_change/base_class.hpp | 3 + .../scene_module/lane_change/normal.hpp | 1 - .../src/scene_module/lane_change/normal.cpp | 84 ++++++++++++++----- 4 files changed, 72 insertions(+), 20 deletions(-) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index da5cc757682c5..97c3104242026 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -14,6 +14,10 @@ behavior_path_planner_avoidance: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance +behavior_path_planner_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change + behavior_velocity_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 26f68e69b3728..472564a061f04 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -271,6 +271,9 @@ class LaneChangeBase mutable LaneChangeTargetObjects debug_filtered_objects_{}; mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change"); + mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index e9e361e1c39b8..ccadcd7144a9b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -173,7 +173,6 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } - rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); double stop_time_{0.0}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 5c149cf0695ba..e3ee24d7d808d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -638,6 +638,8 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { + RCLCPP_DEBUG( + logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -646,6 +648,9 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + RCLCPP_DEBUG( + logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, + max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -654,7 +659,8 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( if (isVehicleStuckByObstacle(current_lanes)) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( - logger_, clock, 1000, "Vehicle is stuck. sample all longitudinal acceleration."); + logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, + max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -664,12 +670,17 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_pose = route_handler.getGoalPose(); if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } + RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -1017,6 +1028,7 @@ bool NormalLaneChange::getLaneChangePaths( { object_debug_.clear(); if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } const auto & route_handler = *getRouteHandler(); @@ -1052,6 +1064,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } @@ -1063,8 +1076,18 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->reserve( longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); + for (const auto & prepare_duration : prepare_durations) { for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration + << ", lon_acc = " << sampled_longitudinal_acc); + }; + // get path on original lanes const auto prepare_velocity = std::max( current_velocity + sampled_longitudinal_acc * prepare_duration, @@ -1082,7 +1105,7 @@ bool NormalLaneChange::getLaneChangePaths( auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); + debug_print("prepare segment is empty...? Unexpected."); continue; } @@ -1093,8 +1116,7 @@ bool NormalLaneChange::getLaneChangePaths( // Check if the lane changing start point is not on the lanes next to target lanes, if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + debug_print("lane change start getEgoPose() is behind target lanelet!"); break; } @@ -1109,11 +1131,22 @@ bool NormalLaneChange::getLaneChangePaths( common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; + + std::vector sample_lat_acc; constexpr double lateral_acc_epsilon = 0.01; + for (double a = min_lateral_acc; a < max_lateral_acc + lateral_acc_epsilon; + a += lateral_acc_resolution) { + sample_lat_acc.push_back(a); + } + RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); + + for (const auto & lateral_acc : sample_lat_acc) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " + << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); + }; - for (double lateral_acc = min_lateral_acc; - lateral_acc < max_lateral_acc + lateral_acc_epsilon; - lateral_acc += lateral_acc_resolution) { const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); const double longitudinal_acc_on_lane_changing = @@ -1129,7 +1162,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + debug_print("Reject: length of lane changing path is longer than length to goal!!"); continue; } @@ -1147,7 +1180,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + debug_print("Reject: length of lane changing path is longer than length to goal!!"); continue; } } @@ -1157,7 +1190,7 @@ bool NormalLaneChange::getLaneChangePaths( initial_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); + debug_print("Reject: target segment is empty!! something wrong..."); continue; } @@ -1174,7 +1207,9 @@ bool NormalLaneChange::getLaneChangePaths( boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); if (!is_valid_start_point) { - // lane changing points are not inside of the target preferred lanes or its neighbors + debug_print( + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); continue; } @@ -1186,7 +1221,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); + debug_print("Reject: target_lane_reference_path is empty!!"); continue; } @@ -1211,32 +1246,31 @@ bool NormalLaneChange::getLaneChangePaths( sorted_lane_ids); if (!candidate_path) { - RCLCPP_DEBUG(logger_, "no candidate path!!"); + debug_print("Reject: failed to generate candidate path!!"); continue; } if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - RCLCPP_DEBUG(logger_, "invalid candidate path!!"); + debug_print("Reject: invalid candidate path!!"); continue; } if ( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { - RCLCPP_DEBUG(logger_, "Including crosswalk!!"); if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including crosswalk!!"); continue; } - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - RCLCPP_WARN_THROTTLE( - logger_, clock, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); + RCLCPP_INFO_THROTTLE( + logger_, clock_, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); } if ( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { - RCLCPP_DEBUG(logger_, "Including intersection!!"); if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including intersection!!"); continue; } RCLCPP_WARN_STREAM( @@ -1249,10 +1283,14 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::passParkedObject( route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, is_goal_in_route, *lane_change_parameters_, object_debug_)) { + debug_print( + "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " + "lane change."); return false; } if (!check_safety) { + debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); return false; } @@ -1260,12 +1298,16 @@ bool NormalLaneChange::getLaneChangePaths( *candidate_path, target_objects, rss_params, is_stuck, object_debug_); if (is_safe) { + debug_print("ACCEPT!!!: it is valid and safe!"); return true; } + + debug_print("Reject: sampled path is not safe."); } } } + RCLCPP_DEBUG(logger_, "No safety path found."); return false; } @@ -1626,11 +1668,13 @@ bool NormalLaneChange::isVehicleStuckByObstacle( { // Ego is still moving, not in stuck if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); return false; } // Ego is just stopped, not sure it is in stuck yet. if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); return false; } @@ -1648,11 +1692,13 @@ bool NormalLaneChange::isVehicleStuckByObstacle( const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { + RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); return true; // Stationary object is in front of ego. } } // No stationary objects found in obstacle_check_distance + RCLCPP_DEBUG(logger_, "No stationary objects found in obstacle_check_distance"); return false; } From dfa43da4a054a23fd93a1acb7309de97b4ce79e2 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 13 Oct 2023 15:43:51 +0900 Subject: [PATCH 11/14] fix(behavior_path_planner): fix calcMinimumLongitudinalLength args (#5299) Signed-off-by: kosuke55 --- .../utils/path_safety_checker/safety_check.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8d8abccbd5413..d78c995aa867a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -76,7 +76,7 @@ double calcRssDistance( double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); boost::optional calcInterpolatedPoseWithVelocity( const std::vector & path, const double relative_time); From b2a5c3f9ec874400a89390d2b8ca526622004fbe Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 16 Oct 2023 15:10:49 +0900 Subject: [PATCH 12/14] fix(lane_change): fix filtering objects Signed-off-by: kosuke55 --- .../scene_module/lane_change/normal.hpp | 3 ++- .../src/scene_module/lane_change/normal.cpp | 22 +++++++++---------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index ccadcd7144a9b..8400cf8c40afd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -151,7 +151,8 @@ class NormalLaneChange : public LaneChangeBase CollisionCheckDebugMap & debug_data) const; LaneChangeTargetObjectIndices filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index e3ee24d7d808d..e9c3bf903f9ae 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -728,7 +728,9 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; + auto objects = *planner_data_->dynamic_object; + utils::path_safety_checker::filterObjectsByClass( + objects, lane_change_parameters_->object_types_to_check); // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; @@ -736,7 +738,8 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); + const auto target_obj_index = + filterObject(objects, current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -768,13 +771,13 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( } LaneChangeTargetObjectIndices NormalLaneChange::filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; // Guard if (objects.objects.empty()) { @@ -814,15 +817,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( target_backward_polygons.push_back(lane_polygon); } - auto filtered_objects = objects; - - utils::path_safety_checker::filterObjectsByClass( - filtered_objects, lane_change_parameters_->object_types_to_check); - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < filtered_objects.objects.size(); ++i) { - const auto & object = filtered_objects.objects.at(i); - const auto & obj_velocity_norm = std::hypot( + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto extended_object = From 29af1265cbb97da123309b82e74c1470e6b7eabb Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 16 Oct 2023 17:20:02 +0900 Subject: [PATCH 13/14] fix(lane_change): filter out objects out of lane to fix stopping margin chattering Signed-off-by: kosuke55 use boost intersects Signed-off-by: kosuke55 --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index e9c3bf903f9ae..32448784f5702 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -278,6 +278,16 @@ void NormalLaneChange::insertStopPoint( if (v > lane_change_parameters_->stop_velocity_threshold) { return false; } + + // target_objects includes objects out of target lanes, so filter them out + if (!boost::geometry::intersects( + tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + lanelet::utils::combineLaneletsShape(status_.target_lanes) + .polygon2d() + .basicPolygon())) { + return false; + } + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); return stopping_distance_for_obj < distance_to_target_lane_obj && distance_to_target_lane_obj < distance_to_ego_lane_obj; From c1838e36291ed18fa16e0ce8f2ded9163368b131 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 16 Oct 2023 18:11:07 +0900 Subject: [PATCH 14/14] fix(lane_change): do not use reference values of polygon outer (#5318) * fix(lane_change): do not use refelence values of polygon outer Signed-off-by: Fumiya Watanabe * fix(lane_change): fix in lane change Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 3 ++- .../src/utils/path_safety_checker/safety_check.cpp | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 32448784f5702..90969ca52b068 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -841,7 +841,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // calc distance from the current ego position double max_dist_ego_to_obj = std::numeric_limits::lowest(); double min_dist_ego_to_obj = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 778d999b011b0..0d4561a29a0f4 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -39,7 +39,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { return true; @@ -58,7 +59,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; @@ -122,7 +124,8 @@ Polygon2d createExtendedPolygon( double min_x = std::numeric_limits::max(); double max_y = std::numeric_limits::lowest(); double min_y = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose);