diff --git a/.github/CODEOWNERS b/.github/_CODEOWNERS similarity index 100% rename from .github/CODEOWNERS rename to .github/_CODEOWNERS diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 3b80314bae5a8..e62f33e085038 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -324,6 +324,9 @@ class RouteHandler lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; + Pose get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const; + private: // MUST lanelet::routing::RoutingGraphPtr routing_graph_ptr_; diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index d534ecc4c84a8..9c60d22972df4 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -46,6 +46,8 @@ namespace autoware::route_handler { namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; @@ -2225,4 +2227,29 @@ std::optional RouteHandler::findDrivableLanePath( if (route) return route->shortestPath(); return {}; } + +Pose RouteHandler::get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const +{ + double accumulated_distance2d = 0; + for (const auto & llt : lanelet_sequence) { + const auto & centerline = llt.centerline(); + for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) { + const auto pt = *it; + const auto next_pt = *std::next(it); + const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt)); + if (accumulated_distance2d + distance2d > s) { + const double ratio = (s - accumulated_distance2d) / distance2d; + const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio; + const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x()); + Pose pose; + pose.position = createPoint(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + pose.orientation = createQuaternionFromYaw(yaw); + return pose; + } + accumulated_distance2d += distance2d; + } + } + return Pose{}; +} } // namespace autoware::route_handler diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index fffb86767b0a8..d47f76e399b4c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -44,11 +44,14 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( { } +// cppcheck-suppress unusedFunction bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && module_type_->isValidPath(); } + +// cppcheck-suppress unusedFunction void AvoidanceByLaneChangeInterface::processOnEntry() { waitApproval(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ddcfda50d18c4..040b71b7b380b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -187,8 +187,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) avoidance_parameters_ = std::make_shared(p); } -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +// cppcheck-suppress unusedFunction +SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp index 95609a430ac80..44f842a67c236 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::SceneModuleInterface; +using SMIPtr = std::unique_ptr; + class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { public: @@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override; + SMIPtr createNewSceneModuleInstance() override; private: std::shared_ptr avoidance_parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 86810789c20ae..a5fdf5a9f0bdf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -34,14 +34,55 @@ #include #include +namespace +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 p; + p.x = static_cast(pose.position.x); + p.y = static_cast(pose.position.y); + p.z = static_cast(pose.position.z); + return p; +}; + +geometry_msgs::msg::Polygon create_execution_area( + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & pose, double additional_lon_offset, double additional_lat_offset) +{ + 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; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + additional_lon_offset; + const double backward_lon_offset = -base_to_rear; + const double lat_offset = width / 2.0 + additional_lat_offset; + + const auto p1 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + geometry_msgs::msg::Polygon polygon; + + polygon.points.push_back(create_point32(p1)); + polygon.points.push_back(create_point32(p2)); + polygon.points.push_back(create_point32(p3)); + polygon.points.push_back(create_point32(p4)); + + return polygon; +} +} // namespace + namespace autoware::behavior_path_planner { using autoware::behavior_path_planner::Direction; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::ObjectInfo; using autoware::behavior_path_planner::Point2d; -using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea; -namespace utils = autoware::behavior_path_planner::utils; AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, @@ -83,9 +124,9 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto & nearest_object = data.target_objects.front(); const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); - const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); + const auto minimum_lane_change_length = calc_minimum_dist_buffer(); - lane_change_debug_.execution_area = createExecutionArea( + lane_change_debug_.execution_area = create_execution_area( getCommonParam().vehicle_info, getEgoPose(), std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); @@ -274,16 +315,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ return avoidance_helper_->getMinAvoidanceDistance(shift_length); } -double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const +double AvoidanceByLaneChange::calc_minimum_dist_buffer() const { - const auto current_lanes = get_current_lanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "no empty lanes"); - return std::numeric_limits::infinity(); - } - - return utils::lane_change::calculation::calc_minimum_lane_change_length( - getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); + const auto [_, dist_buffer] = utils::lane_change::calculation::calc_lc_length_and_dist_buffer( + common_data_ptr_, get_current_lanes()); + return dist_buffer.min; } double AvoidanceByLaneChange::calcLateralOffset() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp index d6bf6bc29df97..42ae41fa380cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp @@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; double calcMinAvoidanceLength(const ObjectData & nearest_object) const; - double calcMinimumLaneChangeLength() const; + double calc_minimum_dist_buffer() const; double calcLateralOffset() const; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt index 509f38d52dd45..194c56cbdc2ba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -18,6 +18,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME} test/test_behavior_path_planner_node_interface.cpp test/test_lane_change_utils.cpp + # test/test_lane_change_scene.cpp ) target_link_libraries(test_${PROJECT_NAME} diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 5f1c3debabcf8..86d8d1c0c1413 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -24,6 +24,108 @@ The lane change candidate path is divided into two phases: preparation and lane- ![lane-change-phases](./images/lane_change-lane_change_phases.png) +The following chart illustrates the process of sampling candidate paths for lane change. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (Is current lanes, target lanes OR target neighbor lanes polygon empty?) then (yes) + #LightPink:Return prev approved path; + stop +else (no) +endif + +:Get target objects; + +:Calculate prepare phase metrics; + +:Start loop through prepare phase metrics; + +repeat + :Get prepare segment; + + if (Is LC start point outside target lanes range) then (yes) + if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop + else (no) + #LightPink:Return prev approved path; + stop + endif + else (no) + endif + + :Calculate shift phase metrics; + + :Start loop through shift phase metrics; + repeat + :Get candidate path; + note left: Details shown in the next chart + if (Is valid candidate path?) then (yes) + :Store candidate path; + if (Is candidate path safe?) then (yes) + #LightGreen:Return candidate path; + stop + else (no) + endif + else (no) + endif + repeat while (Finished looping shift phase metrics) is (FALSE) + repeat while (Is finished looping prepare phase metrics) is (FALSE) + +if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop +else (no) +endif + +#LightPink:Return prev approved path; +stop + +@enduml +``` + +While the following chart demonstrates the process of generating a valid candidate path. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +:Calculate resample interval; + +:Get reference path from target lanes; + +if (Is reference path empty?) then (yes) + #LightPink:Return; + stop +else (no) +endif + +:Get LC shift line; + +:Set lane change Info; +note left: set information from sampled prepare phase and shift phase metrics\n(duration, length, velocity, and acceleration) + +:Construct candidate path; + +if (Candidate path has enough length?) then (yes) + #LightGreen:Return valid candidate path; + stop +else (no) + #LightPink:Return; + stop +endif + +@enduml +``` + ### Preparation phase The preparation trajectory is the candidate path's first and the straight portion generated along the ego vehicle's current lane. The length of the preparation trajectory is computed as follows. @@ -93,7 +195,7 @@ if (max_lane_change_length > ego's distance to the end of the current lanes.) t stop endif -if (isVehicleStuck(current_lanes)) then (yes) +if (ego is stuck in the current lanes) then (yes) :Return **sampled acceleration values**; stop else (no) @@ -249,6 +351,71 @@ stop @enduml ``` +#### Delay Lane Change Check + +In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle. +To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected. + +1. The distance from object to terminal end is sufficient to perform lane change +2. The distance to object is less than the lane changing length +3. The distance from object to next object is sufficient to perform lane change + +If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked. + +The following flow chart illustrates the delay lane change check. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #White + +start +if (Is target objects, candidate path, OR current lane path empty?) then (yes) + #LightPink:Return false; + stop +else (no) +endif + +:Start checking objects from closest to furthest; +repeat + if (Is distance from object to terminal sufficient) then (yes) + else (no) + #LightPink:Return false; + stop + endif + + if (Is distance to object less than lane changing length) then (yes) + else (no) + if (Is only check parked vehicles and vehicle is not parked) then (yes) + else (no) + if(Is last object OR distance to next object is sufficient) then (yes) + #LightGreen: Return true; + stop + else (no) + endif + endif + endif + repeat while (Is finished checking all objects) is (FALSE) + +#LightPink: Return false; +stop + +@enduml +``` + +The following figures demonstrate different situations under which will or will not be triggered: + +1. Delay lane change will be triggered as there is sufficient distance ahead. + ![delay lane change 1](./images/delay_lane_change_1.drawio.svg) +2. Delay lane change will NOT be triggered as there is no sufficient distance ahead + ![delay lane change 2](./images/delay_lane_change_2.drawio.svg) +3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead. + ![delay lane change 3](./images/delay_lane_change_3.drawio.svg) +4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead + ![delay lane change 4](./images/delay_lane_change_4.drawio.svg) +5. Delay lane change will NOT be triggered as there is no sufficient distance ahead. + ![delay lane change 5](./images/delay_lane_change_5.drawio.svg) + #### Candidate Path's Safety check See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) @@ -423,8 +590,6 @@ The ego vehicle may need to secure ample inter-vehicle distance ahead of the tar ![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) -The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. - #### If the lane is blocked and multiple lane changes When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance can be computed from shift length and minimum lane changing velocity. @@ -438,14 +603,65 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) -#### Stopping position when an object exists ahead +### Stopping behavior + +The stopping behavior of the ego vehicle is determined based on various factors, such as the number of lane changes required, the presence of obstacles, and the position of blocking objects in relation to the lane change plan. The objective is to choose a suitable stopping point that allows for a safe and effective lane change while adapting to different traffic scenarios. + +The following flowchart and subsections explain the conditions for deciding where to insert a stop point when an obstacle is ahead. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Inserting Stop Point + +start +if (number of lane changes is zero?) then (YES) +stop +else (NO) +endif + +if (do we want to insert stop point in current lanes?) then (NO) +#LightPink:Insert stop point at next lane change terminal start.; +stop +else (YES) +endif + +if (Is there leading object in the current lanes that blocks ego's path?) then (NO) +#LightPink:Insert stop point at terminal stop.; +stop +else (YES) +endif -When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. -The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case. +if (Blocking object's position is after target lane's start position?) then (NO) +#LightPink:Insert stop at the target lane's start position.; +stop +else (YES) +endif + +if (Blocking object's position is before terminal stop position?) then (NO) +#LightPink:Insert stop at the terminal stop position; +stop +else (YES) +endif + +if (Are there target lane objects between the ego and the blocking object?) then (NO) +#LightGreen:Insert stop behind the blocking object; +stop +else (YES) +#LightPink:Insert stop at terminal stop; +stop +@enduml +``` -##### When the ego vehicle is near the end of the lane change +#### Ego vehicle's stopping position when an object exists ahead -Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. +When the ego vehicle encounters an obstacle ahead, it stops while maintaining a safe distance to prepare for a possible lane change. The exact stopping position depends on factors like whether the target lane is clear or if the lane change needs to be delayed. The following explains how different stopping scenarios are handled: + +##### When the near the end of the lane change + +Whether the target lane has obstacles or is clear, the ego vehicle stops while keeping a safe distance from the obstacle ahead, ensuring there is enough room for the lane change. ![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) @@ -453,20 +669,40 @@ Regardless of the presence or absence of objects in the lane change target lane, ##### When the ego vehicle is not near the end of the lane change -If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. +The ego vehicle stops while maintaining a safe distance from the obstacle ahead, ensuring there is enough space for a lane change. + +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) + +#### Ego vehicle's stopping position when an object exists in the lane changing section + +If there are objects within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without maintaining the usual distance for a lane change. + +##### When near the end of the lane change + +Regardless of whether there are obstacles in the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead, allowing for the lane change. + +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) + +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) + +##### When not near the end of the lane change + +If there are no obstacles in the lane change section of the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead to accommodate the lane change. ![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) -If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. +If there are obstacles within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without keeping the usual distance needed for a lane change. ![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) -##### When the target lane is far away +#### When the target lane is far away -When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. +If the target lane for the lane change is far away and not next to the current lane, the ego vehicle stops closer to the obstacle ahead, as maintaining the usual distance for a lane change is not necessary. ![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) + ### Lane Change When Stuck The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: @@ -647,24 +883,22 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | +| `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | +| `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `trajectory.minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `trajectory.lon_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | +| `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | -| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | -| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | -| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | +| `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | +| `lateral_acceleration.max_values` | [m/s2] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | ### Parameter to judge if lane change is completed @@ -691,6 +925,15 @@ The following parameters are used to judge lane change completion. | `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | | `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | +### Delay Lane Change + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------ | ---- | ------ | ----------------------------------------------------------------------------------------------------- | ------------- | +| `delay_lane_change.enable` | [-] | bool | Flag to enable/disable lane change delay feature | true | +| `delay_lane_change.check_only_parked_vehicle` | [-] | bool | Flag to limit delay feature for only parked vehicles | false | +| `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | +| `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | + ### Collision checks #### Target Objects @@ -717,14 +960,14 @@ The following parameters are used to judge lane change completion. | Name | Unit | Type | Description | Default value | | :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | -| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | -| `enable_collision_check_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | -| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | -| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | -| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | -| `safety_check.collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.enable_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `collision_check.enable_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | +| `collision_check.enable_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | +| `collision_check.check_current_lanes` | [-] | boolean | If true, the lane change module always checks objects in the current lanes for collision assessment. If false, it only checks objects in the current lanes when the ego vehicle is stuck. | false | +| `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | +| `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### safety constraints during lane change path is computed @@ -736,7 +979,7 @@ The following parameters are used to judge lane change completion. | `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | | `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | #### safety constraints specifically for stopped or parked vehicles diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 9fe9dfd62eece..731bb9059768f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -1,40 +1,40 @@ /**: ros__parameters: lane_change: - backward_lane_length: 200.0 #[m] - prepare_duration: 4.0 # [s] - + backward_lane_length: 200.0 backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] - - lane_changing_lateral_jerk: 0.5 # [m/s3] - - minimum_lane_changing_velocity: 2.78 # [m/s] - prediction_time_resolution: 0.5 # [s] - longitudinal_acceleration_sampling_num: 5 - lateral_acceleration_sampling_num: 3 - - # side walk parked vehicle - object_check_min_road_shoulder_width: 0.5 # [m] - object_shiftable_ratio_threshold: 0.6 + backward_length_from_intersection: 5.0 # [m] # turn signal min_length_for_turn_signal_activation: 10.0 # [m] - length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position) - - # longitudinal acceleration - min_longitudinal_acc: -1.0 - max_longitudinal_acc: 1.0 - skip_process: - longitudinal_distance_diff_threshold: - prepare: 0.5 - lane_changing: 0.5 + # trajectory generation + trajectory: + max_prepare_duration: 4.0 + min_prepare_duration: 2.0 + lateral_jerk: 0.5 + min_longitudinal_acc: -1.0 + max_longitudinal_acc: 1.0 + th_prepare_length_diff: 1.0 + th_lane_changing_length_diff: 1.0 + min_lane_changing_velocity: 2.78 + lon_acc_sampling_num: 5 + lat_acc_sampling_num: 3 + lane_changing_decel_factor: 0.5 + + # delay lane change + delay_lane_change: + enable: true + check_only_parked_vehicle: false + min_road_shoulder_width: 0.5 # [m] + th_parked_vehicle_shift_ratio: 0.6 # safety check safety_check: allow_loose_check_for_cancel: true - collision_check_yaw_diff_threshold: 3.1416 + enable_target_lane_bound_check: true + stopped_object_velocity_threshold: 1.0 # [m/s] execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -102,14 +102,16 @@ stop_time: 3.0 # [s] # collision check - enable_collision_check_for_prepare_phase: - general_lanes: false - intersection: true - turns: true - stopped_object_velocity_threshold: 1.0 # [m/s] - check_objects_on_current_lanes: true - check_objects_on_other_lanes: true - use_all_predicted_path: true + collision_check: + enable_for_prepare_phase: + general_lanes: false + intersection: true + turns: true + prediction_time_resolution: 0.5 + yaw_diff_threshold: 3.1416 + check_current_lanes: false + check_other_lanes: false + use_all_predicted_paths: false # lane change cancel cancel: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg new file mode 100644 index 0000000000000..f5f3e4e88559b --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg @@ -0,0 +1,657 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg new file mode 100644 index 0000000000000..8237ac312aadb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg @@ -0,0 +1,625 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg new file mode 100644 index 0000000000000..2bcbfb5cdb93d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg new file mode 100644 index 0000000000000..b38092183db14 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg new file mode 100644 index 0000000000000..d4abb53a84999 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg @@ -0,0 +1,677 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp similarity index 87% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index b237368692312..5317b94db2d6b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -11,12 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/debug.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" @@ -35,6 +35,7 @@ #include #include +class TestNormalLaneChange; namespace autoware::behavior_path_planner { using autoware::route_handler::Direction; @@ -67,6 +68,8 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; + virtual void update_transient_data(const bool is_approved) = 0; + virtual void update_filtered_objects() = 0; virtual void updateLaneChangeStatus() = 0; @@ -110,10 +113,6 @@ class LaneChangeBase virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approve_path_safety_status) = 0; - virtual bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const = 0; - virtual bool isStoppedAtRedTrafficLight() const = 0; virtual bool calcAbortPath() = 0; @@ -129,7 +128,7 @@ class LaneChangeBase virtual void updateSpecialData() {} - virtual void insertStopPoint( + virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, [[maybe_unused]] PathWithLaneId & path) { @@ -236,17 +235,13 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const = 0; + virtual bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; - virtual lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; - virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const @@ -272,20 +267,29 @@ class LaneChangeBase return turn_signal; } - LaneChangeStatus status_{}; + void set_signal_activation_time(const bool reset = false) const + { + if (reset) { + signal_activation_time_ = std::nullopt; + } else if (!signal_activation_time_) { + signal_activation_time_ = clock_.now(); + } + } + + LaneChangeStatus status_; PathShifter path_shifter_{}; LaneChangeStates current_lane_change_state_{}; - std::shared_ptr lane_change_parameters_{}; - std::shared_ptr abort_path_{}; - std::shared_ptr planner_data_{}; - lane_change::CommonDataPtr common_data_ptr_{}; - FilteredByLanesExtendedObjects filtered_objects_{}; + std::shared_ptr lane_change_parameters_; + std::shared_ptr abort_path_; + std::shared_ptr planner_data_; + lane_change::CommonDataPtr common_data_ptr_; + FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; - PathWithLaneId prev_approved_path_{}; + PathWithLaneId prev_approved_path_; int unsafe_hysteresis_count_{0}; bool is_abort_path_approved_{false}; @@ -297,11 +301,14 @@ class LaneChangeBase mutable StopWatch stop_watch_; mutable lane_change::Debug lane_change_debug_; + mutable std::optional signal_activation_time_{std::nullopt}; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; mutable std::shared_ptr time_keeper_; + + friend class ::TestNormalLaneChange; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index fd9375c7e9f75..ee6d27e2edd11 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#include "autoware/behavior_path_lane_change_module/base_class.hpp" #include "autoware/behavior_path_lane_change_module/scene.hpp" -#include "autoware/behavior_path_lane_change_module/utils/base_class.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -94,6 +94,8 @@ class LaneChangeInterface : public SceneModuleInterface MarkerArray getModuleVirtualWall() override; protected: + using SceneModuleInterface::updateRTCStatus; + std::shared_ptr parameters_; std::unique_ptr module_type_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp index 6e6d267dc445f..124579735e5d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "autoware/route_handler/route_handler.hpp" @@ -27,6 +27,7 @@ namespace autoware::behavior_path_planner { +using autoware::behavior_path_planner::lane_change::LCParamPtr; using autoware::route_handler::Direction; class LaneChangeModuleManager : public SceneModuleManagerInterface @@ -44,6 +45,8 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + static LCParamPtr set_params(rclcpp::Node * node, const std::string & node_name); + protected: void initParams(rclcpp::Node * node); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 1bf8407d64ffb..9bf66722dcec2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -14,8 +14,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/base_class.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/base_class.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include #include @@ -54,6 +54,8 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; + void update_transient_data(const bool is_approved) final; + void update_filtered_objects() final; void updateLaneChangeStatus() override; @@ -68,7 +70,9 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + + void insert_stop_point_on_current_lanes(PathWithLaneId & path); PathWithLaneId getReferencePath() const override; @@ -87,10 +91,6 @@ class NormalLaneChange : public LaneChangeBase bool isRequiredStop(const bool is_trailing_object) override; - bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const override; - bool hasFinishedLaneChange() const override; bool isAbleToReturnCurrentLane() const override; @@ -114,57 +114,42 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() const final; protected: - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const override; + lanelet::ConstLanelets get_lane_change_lanes(const lanelet::ConstLanelets & current_lanes) const; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; TurnSignalInfo get_terminal_turn_signal_info() const final; - std::vector sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; - - std::vector calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; - - lane_change::TargetObjects getTargetObjects( - const FilteredByLanesExtendedObjects & predicted_objects, + lane_change::TargetObjects get_target_objects( + const FilteredLanesObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; - FilteredByLanesExtendedObjects filterObjects() const; + FilteredLanesObjects filter_objects() const; void filterOncomingObjects(PredictedObjects & objects) const; - FilteredByLanesObjects filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const override; + bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const override; PathWithLaneId getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + std::vector get_prepare_metrics() const; + std::vector get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, + const double shift_length, const double dist_to_reg_element) const; - bool hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - bool hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + LaneChangePath get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double shift_length) const; - bool hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; - - bool getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const; + bool check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, @@ -189,15 +174,9 @@ class NormalLaneChange : public LaneChangeBase const RSSparams & selected_rss_param, const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) 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 isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; - double get_max_velocity_for_safety_check() const; - bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool is_ego_stuck() const; /** * @brief Checks if the given pose is a valid starting point for a lane change. @@ -219,12 +198,7 @@ class NormalLaneChange : public LaneChangeBase bool check_prepare_phase() 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 set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); void updateStopTime(); @@ -235,6 +209,9 @@ class NormalLaneChange : public LaneChangeBase return common_data_ptr_->lanes_ptr->target; } + void update_dist_from_intersection(); + + std::vector path_after_intersection_; double stop_time_{0.0}; static constexpr double floating_err_th{1e-3}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp new file mode 100644 index 0000000000000..26044924e5fb4 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -0,0 +1,298 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ + +#include "autoware/behavior_path_lane_change_module/structs/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner::lane_change +{ +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using route_handler::Direction; +using route_handler::RouteHandler; +using universe_utils::Polygon2d; +using utils::path_safety_checker::ExtendedPredictedObjects; + +enum class States { + Normal = 0, + Cancel, + Abort, + Stop, +}; + +struct PhaseInfo +{ + double prepare{0.0}; + double lane_changing{0.0}; + + [[nodiscard]] double sum() const { return prepare + lane_changing; } + + PhaseInfo(const double _prepare, const double _lane_changing) + : prepare(_prepare), lane_changing(_lane_changing) + { + } +}; + +struct PhaseMetrics +{ + double duration{0.0}; + double length{0.0}; + double velocity{0.0}; + double sampled_lon_accel{0.0}; + double actual_lon_accel{0.0}; + double lat_accel{0.0}; + + PhaseMetrics() = default; + PhaseMetrics( + const double _duration, const double _length, const double _velocity, + const double _sampled_lon_accel, const double _actual_lon_accel, const double _lat_accel) + : duration(_duration), + length(_length), + velocity(_velocity), + sampled_lon_accel(_sampled_lon_accel), + actual_lon_accel(_actual_lon_accel), + lat_accel(_lat_accel) + { + } +}; + +struct Lanes +{ + bool current_lane_in_goal_section{false}; + bool target_lane_in_goal_section{false}; + lanelet::ConstLanelet ego_lane; + lanelet::ConstLanelets current; + lanelet::ConstLanelets target_neighbor; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + +struct Info +{ + PhaseInfo longitudinal_acceleration{0.0, 0.0}; + PhaseInfo velocity{0.0, 0.0}; + PhaseInfo duration{0.0, 0.0}; + PhaseInfo length{0.0, 0.0}; + + Pose lane_changing_start; + Pose lane_changing_end; + + ShiftLine shift_line; + + double lateral_acceleration{0.0}; + double terminal_lane_changing_velocity{0.0}; + + Info() = default; + Info( + const PhaseMetrics & _prep_metrics, const PhaseMetrics & _lc_metrics, + const Pose & _lc_start_pose, const Pose & _lc_end_pose, const ShiftLine & _shift_line) + { + longitudinal_acceleration = + PhaseInfo{_prep_metrics.actual_lon_accel, _lc_metrics.actual_lon_accel}; + duration = PhaseInfo{_prep_metrics.duration, _lc_metrics.duration}; + velocity = PhaseInfo{_prep_metrics.velocity, _prep_metrics.velocity}; + length = PhaseInfo{_prep_metrics.length, _lc_metrics.length}; + lane_changing_start = _lc_start_pose; + lane_changing_end = _lc_end_pose; + lateral_acceleration = _lc_metrics.lat_accel; + terminal_lane_changing_velocity = _lc_metrics.velocity; + shift_line = _shift_line; + } +}; + +struct TargetLaneLeadingObjects +{ + ExtendedPredictedObjects moving; + ExtendedPredictedObjects stopped; + + // for objects outside of target lanes, but close to its boundaries + ExtendedPredictedObjects stopped_at_bound; + + [[nodiscard]] size_t size() const + { + return moving.size() + stopped.size() + stopped_at_bound.size(); + } +}; + +struct FilteredLanesObjects +{ + ExtendedPredictedObjects others; + ExtendedPredictedObjects current_lane; + ExtendedPredictedObjects target_lane_trailing; + TargetLaneLeadingObjects target_lane_leading; +}; + +struct TargetObjects +{ + ExtendedPredictedObjects leading; + ExtendedPredictedObjects trailing; + TargetObjects(ExtendedPredictedObjects leading, ExtendedPredictedObjects trailing) + : leading(std::move(leading)), trailing(std::move(trailing)) + { + } +}; + +enum class ModuleType { + NORMAL = 0, + EXTERNAL_REQUEST, + AVOIDANCE_BY_LANE_CHANGE, +}; + +struct PathSafetyStatus +{ + bool is_safe{true}; + bool is_trailing_object{false}; + + PathSafetyStatus() = default; + PathSafetyStatus(const bool is_safe, const bool is_trailing_object) + : is_safe(is_safe), is_trailing_object(is_trailing_object) + { + } +}; + +struct LanesPolygon +{ + lanelet::BasicPolygon2d current; + lanelet::BasicPolygon2d target; + lanelet::BasicPolygon2d expanded_target; + lanelet::BasicPolygon2d target_neighbor; + std::vector preceding_target; +}; + +struct MinMaxValue +{ + double min{std::numeric_limits::infinity()}; + double max{std::numeric_limits::infinity()}; +}; + +struct TransientData +{ + Polygon2d current_footprint; // ego's polygon at current pose + + MinMaxValue lane_changing_length; // lane changing length for a single lane change + MinMaxValue + current_dist_buffer; // distance buffer computed backward from current lanes' terminal end + MinMaxValue + next_dist_buffer; // distance buffer computed backward from target lanes' terminal end + double dist_to_terminal_end{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal end + double dist_from_prev_intersection{std::numeric_limits::max()}; + // terminal end + double dist_to_terminal_start{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal start + double max_prepare_length{ + std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + + double target_lane_length{std::numeric_limits::min()}; + + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes + lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes + + size_t current_path_seg_idx; // index of nearest segment to ego along current path + double current_path_velocity; // velocity of the current path at the ego position along the path + + double lane_change_prepare_duration{0.0}; + + bool is_ego_near_current_terminal_start{false}; + bool is_ego_stuck{false}; + + bool in_turn_direction_lane{false}; + bool in_intersection{false}; +}; + +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; +using LanesPolygonPtr = std::shared_ptr; + +struct CommonData +{ + RouteHandlerPtr route_handler_ptr; + Odometry::ConstSharedPtr self_odometry_ptr; + BppParamPtr bpp_param_ptr; + LCParamPtr lc_param_ptr; + LanesPtr lanes_ptr; + LanesPolygonPtr lanes_polygon_ptr; + TransientData transient_data; + PathWithLaneId current_lanes_path; + ModuleType lc_type; + Direction direction; + + [[nodiscard]] const Pose & get_ego_pose() const { return self_odometry_ptr->pose.pose; } + + [[nodiscard]] const Twist & get_ego_twist() const { return self_odometry_ptr->twist.twist; } + + [[nodiscard]] double get_ego_speed(bool use_norm = false) const + { + if (!use_norm) { + return get_ego_twist().linear.x; + } + + const auto x = get_ego_twist().linear.x; + const auto y = get_ego_twist().linear.y; + return std::hypot(x, y); + } + + [[nodiscard]] bool is_data_available() const + { + return route_handler_ptr && self_odometry_ptr && bpp_param_ptr && lc_param_ptr && lanes_ptr && + lanes_polygon_ptr; + } + + [[nodiscard]] bool is_lanes_available() const + { + return lanes_ptr && !lanes_ptr->current.empty() && !lanes_ptr->target.empty() && + !lanes_ptr->target_neighbor.empty(); + } +}; +using CommonDataPtr = std::shared_ptr; +} // namespace autoware::behavior_path_planner::lane_change + +namespace autoware::behavior_path_planner +{ +using autoware_perception_msgs::msg::PredictedObject; +using utils::path_safety_checker::ExtendedPredictedObjects; +using LaneChangeModuleType = lane_change::ModuleType; +using LaneChangeParameters = lane_change::Parameters; +using LaneChangeStates = lane_change::States; +using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangePhaseMetrics = lane_change::PhaseMetrics; +using LaneChangeInfo = lane_change::Info; +using FilteredLanesObjects = lane_change::FilteredLanesObjects; +using LateralAccelerationMap = lane_change::LateralAccelerationMap; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp similarity index 80% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index fc51a82a8a842..ea9807ad1616f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -11,11 +11,11 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include @@ -35,7 +35,7 @@ struct Debug LaneChangePaths valid_paths; CollisionCheckDebugMap collision_check_objects; CollisionCheckDebugMap collision_check_objects_after_approval; - FilteredByLanesExtendedObjects filtered_objects; + FilteredLanesObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; geometry_msgs::msg::Pose ego_pose; lanelet::ConstLanelets current_lanes; @@ -55,9 +55,11 @@ struct Debug collision_check_objects.clear(); collision_check_objects_after_approval.clear(); filtered_objects.current_lane.clear(); - filtered_objects.target_lane_leading.clear(); filtered_objects.target_lane_trailing.clear(); - filtered_objects.other_lane.clear(); + filtered_objects.target_lane_leading.moving.clear(); + filtered_objects.target_lane_leading.stopped.clear(); + filtered_objects.target_lane_leading.stopped_at_bound.clear(); + filtered_objects.others.clear(); execution_area.points.clear(); current_lanes.clear(); target_lanes.clear(); @@ -74,4 +76,4 @@ struct Debug }; } // namespace autoware::behavior_path_planner::lane_change -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp new file mode 100644 index 0000000000000..7e0beea10a91e --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -0,0 +1,180 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ + +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner::lane_change +{ +using utils::path_safety_checker::ObjectTypesToCheck; +using utils::path_safety_checker::RSSparams; + +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; + +struct CancelParameters +{ + bool enable_on_prepare_phase{true}; + bool enable_on_lane_changing_phase{false}; + double delta_time{1.0}; + double duration{5.0}; + double max_lateral_jerk{10.0}; + double overhang_tolerance{0.0}; + + // th_unsafe_hysteresis will be compare with the number of detected unsafe instance. If the + // number of unsafe exceeds th_unsafe_hysteresis, the lane change will be cancelled or + // aborted. + int th_unsafe_hysteresis{2}; + + int deceleration_sampling_num{5}; +}; + +struct CollisionCheckParameters +{ + bool enable_for_prepare_phase_in_general_lanes{false}; + bool enable_for_prepare_phase_in_intersection{true}; + bool enable_for_prepare_phase_in_turns{true}; + bool check_current_lane{true}; + bool check_other_lanes{true}; + bool use_all_predicted_paths{false}; + double th_yaw_diff{3.1416}; + double prediction_time_resolution{0.5}; +}; + +struct SafetyParameters +{ + bool enable_loose_check_for_cancel{true}; + bool enable_target_lane_bound_check{true}; + double th_stopped_object_velocity{0.1}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; + RSSparams rss_params{}; + RSSparams rss_params_for_parked{}; + RSSparams rss_params_for_abort{}; + RSSparams rss_params_for_stuck{}; + ObjectTypesToCheck target_object_types{}; + CollisionCheckParameters collision_check{}; +}; + +struct TrajectoryParameters +{ + double max_prepare_duration{4.0}; + double min_prepare_duration{1.0}; + double lateral_jerk{0.5}; + double min_longitudinal_acc{-1.0}; + double max_longitudinal_acc{1.0}; + double th_prepare_length_diff{0.5}; + double th_lane_changing_length_diff{0.5}; + double min_lane_changing_velocity{5.6}; + double lane_changing_decel_factor{0.5}; + int lon_acc_sampling_num{10}; + int lat_acc_sampling_num{10}; + LateralAccelerationMap lat_acc_map{}; +}; + +struct DelayParameters +{ + bool enable{true}; + bool check_only_parked_vehicle{false}; + double min_road_shoulder_width{0.5}; + double th_parked_vehicle_shift_ratio{0.6}; +}; + +struct Parameters +{ + TrajectoryParameters trajectory{}; + SafetyParameters safety{}; + CancelParameters cancel{}; + DelayParameters delay{}; + + // lane change parameters + double backward_lane_length{200.0}; + double backward_length_buffer_for_end_of_lane{0.0}; + double backward_length_buffer_for_blocking_object{0.0}; + double backward_length_from_intersection{5.0}; + + // parked vehicle + double object_check_min_road_shoulder_width{0.5}; + double th_object_shiftable_ratio{0.6}; + + // turn signal + double min_length_for_turn_signal_activation{10.0}; + + // regulatory elements + bool regulate_on_crosswalk{false}; + bool regulate_on_intersection{false}; + bool regulate_on_traffic_light{false}; + + // ego vehicle stuck detection + double th_stop_velocity{0.1}; + double th_stop_time{3.0}; + + // finish judge parameter + double lane_change_finish_judge_buffer{3.0}; + double th_finish_judge_lateral_diff{0.2}; + double th_finish_judge_yaw_diff{autoware_utils::deg2rad(3.0)}; + + // debug marker + bool publish_debug_marker{false}; +}; + +} // namespace autoware::behavior_path_planner::lane_change + +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp similarity index 78% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 5623f03a22eb3..0fa0a9d977a26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -29,19 +29,18 @@ using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; struct Path { - PathWithLaneId path{}; - ShiftedPath shifted_path{}; - Info info{}; + PathWithLaneId path; + ShiftedPath shifted_path; + Info info; }; struct Status { - Path lane_change_path{}; + Path lane_change_path; bool is_safe{false}; bool is_valid_path{false}; double start_distance{0.0}; }; - } // namespace autoware::behavior_path_planner::lane_change namespace autoware::behavior_path_planner @@ -50,4 +49,4 @@ using LaneChangePath = lane_change::Path; using LaneChangePaths = std::vector; using LaneChangeStatus = lane_change::Status; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index f395902dcc476..1312ea06c4435 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include @@ -24,23 +24,10 @@ using autoware::route_handler::Direction; using autoware::route_handler::RouteHandler; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LCParamPtr; +using behavior_path_planner::lane_change::MinMaxValue; +using behavior_path_planner::lane_change::PhaseMetrics; -/** - * @brief Calculates the distance from the ego vehicle to the terminal point. - * - * This function computes the distance from the current position of the ego vehicle - * to either the goal pose or the end of the current lane, depending on whether - * the vehicle's current lane is within the goal section. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non null `lanes_ptr` that points to the current lanes data. - * - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle. - * - Non null `route_handler_ptr` that contains the goal pose of the route. - * - * @return The distance to the terminal point (either the goal pose or the end of the current lane) - * in meters. - */ -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr); +static constexpr double eps = 0.001; double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, @@ -63,6 +50,23 @@ double calc_dist_from_pose_to_terminal_end( */ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); +/** + * @brief Calculates the distance to last fit width position along the lane + * + * This function computes the distance from the current ego position to the last + * position along the lane where the ego foot prints stays within the lane + * boundaries. + * + * @param lanelets current ego lanelets + * @param src_pose source pose to calculate distance from + * @param bpp_param common parameters used in behavior path planner. + * @param margin additional margin for checking lane width + * @return distance to last fit width position along the lane + */ +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); + /** * @brief Calculates the maximum preparation longitudinal distance for lane change. * @@ -71,7 +75,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle. * * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change + * - `lc_param_ptr->maximum_prepare_duration`: The maximum duration allowed for lane change * preparation. * - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle. * @@ -101,20 +105,89 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); +double calc_lane_changing_acceleration( + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration); + +std::vector calc_lon_acceleration_samples( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration); + +/** + * @brief calculates the actual prepare duration that will be used for path generation + * + * @details computes the required prepare duration to be used for candidate path + * generation based on the elapsed time of turn signal activation, the minimum + * and maximum parameterized values for the prepare duration, + * and the minimum lane changing velocity. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes + * lane change parameters and bpp common parameters. + * @param current_velocity current ego vehicle velocity. + * @param active_signal_duration elapsed time since turn signal activation. + * @return The calculated prepare duration value in seconds (s) + */ +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration); -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction); +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double max_path_velocity, const double min_length_threshold = 0.0, + const double max_length_threshold = std::numeric_limits::max()); -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc); +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_path_velocity, const double lon_accel, + const double max_length_threshold = std::numeric_limits::max()); -double calc_maximum_lane_change_length( - const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, - const double max_acc); +/** + * @brief Calculates the minimum and maximum lane changing lengths, along with the corresponding + * distance buffers. + * + * This function computes the minimum and maximum lane change lengths based on shift intervals and + * vehicle parameters. It only accounts for the lane changing phase itself, excluding the prepare + * distance, which should be handled separately during path generation. + * + * Additionally, the function calculates the distance buffer to ensure that the ego vehicle has + * enough space to complete the lane change, including cases where multiple lane changes may be + * necessary. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes: + * - `lc_param_ptr`: Parameters related to lane changing, such as velocity, lateral acceleration, + * and jerk. + * - `route_handler_ptr`: Pointer to the route handler for managing routes. + * - `direction`: Lane change direction (e.g., left or right). + * - `transient_data.acc.max`: Maximum acceleration of the vehicle. + * - Other relevant data for the ego vehicle's dynamics and state. + * @param lanes The set of lanelets representing the lanes for the lane change. + * + * @return A pair of `MinMaxValue` structures where: + * - The first element contains the minimum and maximum lane changing lengths in meters. + * - The second element contains the minimum and maximum distance buffers in meters. + * If the lanes or necessary data are unavailable, returns `std::numeric_limits::max()` for + * both values. + */ +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp deleted file mode 100644 index 194412dfe01b7..0000000000000 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ /dev/null @@ -1,325 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ - -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" - -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include - -namespace autoware::behavior_path_planner::lane_change -{ -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using nav_msgs::msg::Odometry; -using route_handler::Direction; -using route_handler::RouteHandler; -using utils::path_safety_checker::ExtendedPredictedObjects; - -struct LateralAccelerationMap -{ - std::vector base_vel; - std::vector base_min_acc; - std::vector base_max_acc; - - void add(const double velocity, const double min_acc, const double max_acc) - { - if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { - return; - } - - size_t idx = 0; - for (size_t i = 0; i < base_vel.size(); ++i) { - if (velocity < base_vel.at(i)) { - break; - } - idx = i + 1; - } - - base_vel.insert(base_vel.begin() + idx, velocity); - base_min_acc.insert(base_min_acc.begin() + idx, min_acc); - base_max_acc.insert(base_max_acc.begin() + idx, max_acc); - } - - std::pair find(const double velocity) const - { - if (!base_vel.empty() && velocity < base_vel.front()) { - return std::make_pair(base_min_acc.front(), base_max_acc.front()); - } - if (!base_vel.empty() && velocity > base_vel.back()) { - return std::make_pair(base_min_acc.back(), base_max_acc.back()); - } - - const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); - - return std::make_pair(min_acc, max_acc); - } -}; - -struct CancelParameters -{ - bool enable_on_prepare_phase{true}; - bool enable_on_lane_changing_phase{false}; - double delta_time{1.0}; - double duration{5.0}; - double max_lateral_jerk{10.0}; - double overhang_tolerance{0.0}; - - // unsafe_hysteresis_threshold will be compare with the number of detected unsafe instance. If the - // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or - // aborted. - int unsafe_hysteresis_threshold{2}; - - int deceleration_sampling_num{5}; -}; - -struct Parameters -{ - // trajectory generation - double backward_lane_length{200.0}; - double prediction_time_resolution{0.5}; - int longitudinal_acc_sampling_num{10}; - int lateral_acc_sampling_num{10}; - - // lane change parameters - double backward_length_buffer_for_end_of_lane{0.0}; - double backward_length_buffer_for_blocking_object{0.0}; - double lane_changing_lateral_jerk{0.5}; - double minimum_lane_changing_velocity{5.6}; - double lane_change_prepare_duration{4.0}; - LateralAccelerationMap lane_change_lat_acc_map; - - // parked vehicle - double object_check_min_road_shoulder_width{0.5}; - double object_shiftable_ratio_threshold{0.6}; - - // turn signal - double min_length_for_turn_signal_activation{10.0}; - double length_ratio_for_turn_signal_deactivation{0.8}; - - // acceleration data - double min_longitudinal_acc{-1.0}; - double max_longitudinal_acc{1.0}; - - double skip_process_lon_diff_th_prepare{0.5}; - double skip_process_lon_diff_th_lane_changing{1.0}; - - // collision check - bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; - bool enable_collision_check_for_prepare_phase_in_intersection{true}; - bool enable_collision_check_for_prepare_phase_in_turns{true}; - double stopped_object_velocity_threshold{0.1}; - bool check_objects_on_current_lanes{true}; - bool check_objects_on_other_lanes{true}; - bool use_all_predicted_path{false}; - double lane_expansion_left_offset{0.0}; - double lane_expansion_right_offset{0.0}; - - // regulatory elements - bool regulate_on_crosswalk{false}; - bool regulate_on_intersection{false}; - bool regulate_on_traffic_light{false}; - - // ego vehicle stuck detection - double stop_velocity_threshold{0.1}; - double stop_time_threshold{3.0}; - - // true by default for all objects - utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; - - // safety check - bool allow_loose_check_for_cancel{true}; - double collision_check_yaw_diff_threshold{3.1416}; - utils::path_safety_checker::RSSparams rss_params{}; - utils::path_safety_checker::RSSparams rss_params_for_parked{}; - utils::path_safety_checker::RSSparams rss_params_for_abort{}; - utils::path_safety_checker::RSSparams rss_params_for_stuck{}; - - // abort - CancelParameters cancel{}; - - // finish judge parameter - double lane_change_finish_judge_buffer{3.0}; - double finish_judge_lateral_threshold{0.2}; - double finish_judge_lateral_angle_deviation{autoware::universe_utils::deg2rad(3.0)}; - - // debug marker - bool publish_debug_marker{false}; -}; - -enum class States { - Normal = 0, - Cancel, - Abort, - Stop, -}; - -struct PhaseInfo -{ - double prepare{0.0}; - double lane_changing{0.0}; - - [[nodiscard]] double sum() const { return prepare + lane_changing; } - - PhaseInfo(const double _prepare, const double _lane_changing) - : prepare(_prepare), lane_changing(_lane_changing) - { - } -}; - -struct Lanes -{ - bool current_lane_in_goal_section{false}; - lanelet::ConstLanelets current; - lanelet::ConstLanelets target_neighbor; - lanelet::ConstLanelets target; - std::vector preceding_target; -}; - -struct Info -{ - PhaseInfo longitudinal_acceleration{0.0, 0.0}; - PhaseInfo velocity{0.0, 0.0}; - PhaseInfo duration{0.0, 0.0}; - PhaseInfo length{0.0, 0.0}; - - Pose lane_changing_start{}; - Pose lane_changing_end{}; - - ShiftLine shift_line{}; - - double lateral_acceleration{0.0}; - double terminal_lane_changing_velocity{0.0}; -}; - -template -struct LanesObjects -{ - Object current_lane{}; - Object target_lane_leading{}; - Object target_lane_trailing{}; - Object other_lane{}; - - LanesObjects() = default; - LanesObjects( - Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane) - : current_lane(std::move(current_lane)), - target_lane_leading(std::move(target_lane_leading)), - target_lane_trailing(std::move(target_lane_trailing)), - other_lane(std::move(other_lane)) - { - } -}; - -struct TargetObjects -{ - ExtendedPredictedObjects leading; - ExtendedPredictedObjects trailing; - TargetObjects(ExtendedPredictedObjects leading, ExtendedPredictedObjects trailing) - : leading(std::move(leading)), trailing(std::move(trailing)) - { - } -}; - -enum class ModuleType { - NORMAL = 0, - EXTERNAL_REQUEST, - AVOIDANCE_BY_LANE_CHANGE, -}; - -struct PathSafetyStatus -{ - bool is_safe{true}; - bool is_trailing_object{false}; - - PathSafetyStatus() = default; - PathSafetyStatus(const bool is_safe, const bool is_trailing_object) - : is_safe(is_safe), is_trailing_object(is_trailing_object) - { - } -}; - -struct LanesPolygon -{ - std::optional current; - std::optional target; - std::optional expanded_target; - lanelet::BasicPolygon2d target_neighbor; - std::vector preceding_target; -}; - -using RouteHandlerPtr = std::shared_ptr; -using BppParamPtr = std::shared_ptr; -using LCParamPtr = std::shared_ptr; -using LanesPtr = std::shared_ptr; -using LanesPolygonPtr = std::shared_ptr; - -struct CommonData -{ - RouteHandlerPtr route_handler_ptr; - Odometry::ConstSharedPtr self_odometry_ptr; - BppParamPtr bpp_param_ptr; - LCParamPtr lc_param_ptr; - LanesPtr lanes_ptr; - LanesPolygonPtr lanes_polygon_ptr; - ModuleType lc_type; - Direction direction; - - [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } - - [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } - - [[nodiscard]] double get_ego_speed(bool use_norm = false) const - { - if (!use_norm) { - return get_ego_twist().linear.x; - } - - const auto x = get_ego_twist().linear.x; - const auto y = get_ego_twist().linear.y; - return std::hypot(x, y); - } -}; -using CommonDataPtr = std::shared_ptr; -} // namespace autoware::behavior_path_planner::lane_change - -namespace autoware::behavior_path_planner -{ -using autoware_perception_msgs::msg::PredictedObject; -using utils::path_safety_checker::ExtendedPredictedObjects; -using LaneChangeModuleType = lane_change::ModuleType; -using LaneChangeParameters = lane_change::Parameters; -using LaneChangeStates = lane_change::States; -using LaneChangePhaseInfo = lane_change::PhaseInfo; -using LaneChangeInfo = lane_change::Info; -using FilteredByLanesObjects = lane_change::LanesObjects>; -using FilteredByLanesExtendedObjects = lane_change::LanesObjects; -using LateralAccelerationMap = lane_change::LateralAccelerationMap; -} // namespace autoware::behavior_path_planner - -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index d403e7e1436c7..4c2712f053b13 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/debug.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -29,7 +29,7 @@ namespace marker_utils::lane_change_markers { -using autoware::behavior_path_planner::FilteredByLanesExtendedObjects; +using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; @@ -40,7 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); MarkerArray showFilteredObjects( - const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns); + const FilteredLanesObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index a19508dd11b7f..ce9ab331d90a1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" @@ -44,81 +44,61 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; +using autoware::universe_utils::LineString2d; using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; +using behavior_path_planner::lane_change::TargetLaneLeadingObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; +bool is_mandatory_lane_change(const ModuleType lc_type); + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); -double calcMinimumAcceleration( - const double current_velocity, const double min_longitudinal_acc, - const LaneChangeParameters & lane_change_parameters); - -double calcMaximumAcceleration( - const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); - -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); -std::vector getAccelerationValues( - const double min_acc, const double max_acc, const size_t sampling_num); - std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); -lanelet::ConstLanelets getTargetPreferredLanes( +lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction & direction, - const LaneChangeModuleType & type); - -lanelet::ConstLanelets getTargetNeighborLanes( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const LaneChangeModuleType & type); bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -std::optional constructCandidatePath( +bool path_footprint_exceeds_target_lane_bound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin = 0.1); + +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length); - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, - const double next_lane_change_buffer); +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval); std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, @@ -132,9 +112,9 @@ double getLateralShift(const LaneChangePath & path); CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction); + +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes); std::vector convert_to_predicted_path( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, @@ -151,26 +131,38 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passed_parked_objects( +/** + * @brief Checks if delaying of lane change maneuver is necessary + * + * @details Scans through the provided target objects (assumed to be ordered from closest to + * furthest), and returns true if any of the objects satisfy the following conditions: + * - Not near the end of current lanes + * - There is sufficient distance from object to next one to do lane change + * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects + * which pass isParkedObject() check will be considered. + * + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, + * and transient data. + * @param lane_change_path Candidate lane change path to apply checks on. + * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include + * target lane leading static objects). + * @param object_debug Collision check debug struct to be updated if any of the target objects + * satisfy the conditions. + * @return bool True if conditions to delay lane change are met + */ +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, + const std::vector & target_objects, CollisionCheckDebugMap & object_debug); -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); - -std::optional createPolygon( +lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); ExtendedPredictedObject transform( - const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters); -bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, - const std::optional & lanes_polygon); +bool is_collided_polygons_in_lanelet( + const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon); /** * @brief Generates expanded lanelets based on the given direction and offsets. @@ -210,14 +202,14 @@ rclcpp::Logger getLogger(const std::string & type); * The footprint is determined by the vehicle's pose and its dimensions, including the distance * from the base to the front and rear ends of the vehicle, as well as its width. * - * @param ego_pose The current pose of the ego vehicle. - * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal - * offset, rear overhang, and width. + * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions + * and pose information. * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ -Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info); +Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info); + +Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left); /** * @brief Checks if the given polygon is within an intersection area. @@ -234,7 +226,7 @@ Polygon2d getEgoCurrentFootprint( * * @return bool True if the polygon is within the intersection area, false otherwise. */ -bool isWithinIntersection( +bool is_within_intersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); @@ -251,24 +243,8 @@ bool isWithinIntersection( * @return bool True if the polygon is within a lane designated for turning, false if it is within a * straight lane or no turn direction is specified. */ -bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); - -/** - * @brief Calculates the distance required during a lane change operation. - * - * Used for computing prepare or lane change length based on current and maximum velocity, - * acceleration, and duration, returning the lesser of accelerated distance or distance at max - * velocity. - * - * @param velocity The current velocity of the vehicle in meters per second (m/s). - * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). - * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). - * @param duration The duration of the lane change in seconds (s). - * @return The calculated minimum distance in meters (m). - */ -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double time); +bool is_within_turn_direction_lanes( + const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); @@ -276,35 +252,169 @@ bool is_same_lane_with_prev_iteration( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation); - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & object); bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & object); double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); -ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects); - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, const bool ignore_intersection = false); -} // namespace autoware::behavior_path_planner::utils::lane_change -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); +/** + * @brief Calculates the minimum distance to a stationary object in the current lanes. + * + * This function determines the closest distance from the ego vehicle to a stationary object + * in the current lanes. It checks if the object is within the stopping criteria based on its + * velocity and calculates the distance while accounting for the object's size. Only objects + * positioned after the specified distance to the target lane's start are considered. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the current + * lane. + * @param dist_to_target_lane_start The distance to the start of the target lane from the ego + * vehicle. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return The minimum distance to a stationary object in the current lanes. If no valid object is + * found, returns the maximum possible double value. + */ +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path); + +/** + * @brief Checks if there is an object in the target lane that influences the decision to insert a + * stop point. + * + * This function determines whether any objects exist in the target lane that would affect + * the decision to place a stop point behind a blocking object in the current lane. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for the lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the target + * lane. + * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return true if there is an object in the target lane that influences the stop point decision; + * otherwise, false. + */ +bool has_blocking_target_object( + const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length, + const PathWithLaneId & path); -geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset); -} // namespace autoware::behavior_path_planner::utils::lane_change::debug +/** + * @brief Checks if the ego vehicle has passed any turn direction within an intersection. + * + * This function determines whether the ego vehicle has exited the intersection and + * turn lane area based on its distance from the previous intersection. It considers + * whether the ego vehicle is currently in an intersection and a turn lane. + * + * @param common_data_ptr Shared pointer to CommonData containing the transient data and + * lane-change parameters required for the distance's comparison. + * + * @return true if the ego vehicle has passed the intersection turn direction, false otherwise. + */ +bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr); + +/** + * @brief Retrieves the predicted paths of an object as 2D line strings. + * + * This function transforms each predicted path of an object into a LineString2d, representing + * a 2D sequence of points. Each point in the path is extracted from the predicted path's + * position and converted to a 2D point. + * + * @param object The predicted object whose paths will be converted into 2D line strings. + * + * @return std::vector A vector of 2D line strings representing the predicted paths + * of the object. + */ +std::vector get_line_string_paths(const ExtendedPredictedObject & object); + +/** + * @brief Determines if there is an object in the turn lane that could overtake the ego vehicle. + * + * This function checks for any trailing objects in the turn lane that may attempt to overtake + * the ego vehicle. The check is only applicable if the ego vehicle is still within a certain + * distance from the previous intersection's turn lane. It evaluates whether any of the predicted + * paths or the initial polygon of trailing objects overlap with the target lane polygon. + * + * @param common_data_ptr Shared pointer to CommonData containing lane and polygon information + * for the ego vehicle. + * @param trailing_objects A collection of predicted objects trailing the ego vehicle. + * + * @return true if there is an object in the turn lane with a potential to overtake, false + * otherwise. + */ +bool has_overtaking_turn_lane_object( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); + +/** + * @brief Filters objects based on their positions and velocities relative to the ego vehicle and + * the target lane. + * + * This function evaluates whether an object should be classified as a leading or trailing object + * in the context of a lane change. Objects are filtered based on their lateral distance from + * the ego vehicle, velocity, and whether they are within the target lane or its expanded + * boundaries. + * + * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, + * vehicle dimensions, lane polygons, and behavior parameters. + * @param object An extended predicted object representing a potential obstacle in the environment. + * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the + * current lanes. + * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of + * the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or + * outside boundaries). + * @param trailing_objects Reference to a collection for storing trailing objects. + * + * @return true if the object is classified as either leading or trailing, false otherwise. + */ +bool filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects); + +/** + * @brief Retrieves the preceding lanes for the target lanes while removing overlapping and + * disconnected lanes. + * + * This function identifies all lanes that precede the target lanes based on the ego vehicle's + * current position and a specified backward search length. The resulting preceding lanes are + * filtered to remove lanes that overlap with the current lanes or are not connected to the route. + * + * @param common_data_ptr Shared pointer to commonly used data in lane change module, which contains + * route handler information, lane details, ego vehicle pose, and behavior parameters. + * + * @return A vector of preceding lanelet groups, with each group containing only the connected and + * non-overlapping preceding lanes. + */ +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr); + +/** + * @brief Determines if the object's predicted path overlaps with the given lane polygon. + * + * This function checks whether any of the line string paths derived from the object's predicted + * trajectories intersect or overlap with the specified polygon representing lanes. + * + * @param object The extended predicted object containing predicted trajectories and initial + * polygon. + * @param lanes_polygon A polygon representing the lanes to check for overlaps with the object's + * paths. + * + * @return true if any of the object's predicted paths overlap with the lanes polygon, false + * otherwise. + */ +bool object_path_overlaps_lanes( + const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon); +} // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index 74d11dbcb1e13..7599817df37da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -26,6 +26,7 @@ autoware_rtc_interface autoware_universe_utils pluginlib + range-v3 rclcpp tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 4ee5156f0ab03..e042bdb5868d3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -77,6 +77,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->update_filtered_objects(); + module_type_->update_transient_data(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -144,11 +145,11 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - *prev_approved_path_ = getPreviousModuleOutput().path; - BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); + *prev_approved_path_ = out.path; + + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); @@ -157,7 +158,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + path_reference_ = std::make_shared(out.reference_path); stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 69df2fe14317a..1d2cb00fafe2a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -34,7 +34,7 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) initParams(node); } -void LaneChangeModuleManager::initParams(rclcpp::Node * node) +LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::string & node_name) { using autoware::universe_utils::getOrDeclareParameter; @@ -43,49 +43,70 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation - p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); - p.prediction_time_resolution = - getOrDeclareParameter(*node, parameter("prediction_time_resolution")); - p.longitudinal_acc_sampling_num = - getOrDeclareParameter(*node, parameter("longitudinal_acceleration_sampling_num")); - p.lateral_acc_sampling_num = - getOrDeclareParameter(*node, parameter("lateral_acceleration_sampling_num")); - - // parked vehicle detection - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); - p.object_shiftable_ratio_threshold = - getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); + { + p.trajectory.max_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.max_prepare_duration")); + p.trajectory.min_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.min_prepare_duration")); + p.trajectory.lateral_jerk = + getOrDeclareParameter(*node, parameter("trajectory.lateral_jerk")); + p.trajectory.min_longitudinal_acc = + getOrDeclareParameter(*node, parameter("trajectory.min_longitudinal_acc")); + p.trajectory.max_longitudinal_acc = + getOrDeclareParameter(*node, parameter("trajectory.max_longitudinal_acc")); + p.trajectory.th_prepare_length_diff = + getOrDeclareParameter(*node, parameter("trajectory.th_prepare_length_diff")); + p.trajectory.th_lane_changing_length_diff = + getOrDeclareParameter(*node, parameter("trajectory.th_lane_changing_length_diff")); + p.trajectory.min_lane_changing_velocity = + getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); + p.trajectory.lane_changing_decel_factor = + getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); + p.trajectory.lon_acc_sampling_num = + getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); + p.trajectory.lat_acc_sampling_num = + getOrDeclareParameter(*node, parameter("trajectory.lat_acc_sampling_num")); + + const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.trajectory.min_lane_changing_velocity = std::min( + p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration); + + // validation of trajectory parameters + if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(node_name), + "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " + << p.trajectory.lon_acc_sampling_num + << "Given lateral parameter: " << p.trajectory.lat_acc_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + // lateral acceleration map + const auto lateral_acc_velocity = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = getOrDeclareParameter>( + *node, parameter("lateral_acceleration.min_values")); + const auto max_lateral_acc = getOrDeclareParameter>( + *node, parameter("lateral_acceleration.max_values")); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR( + node->get_logger().get_child(node_name), + "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.trajectory.lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + } // turn signal p.min_length_for_turn_signal_activation = getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); - p.length_ratio_for_turn_signal_deactivation = - getOrDeclareParameter(*node, parameter("length_ratio_for_turn_signal_deactivation")); - - // acceleration - p.min_longitudinal_acc = getOrDeclareParameter(*node, parameter("min_longitudinal_acc")); - p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); - - // collision check - p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( - *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); - p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( - *node, parameter("enable_collision_check_for_prepare_phase.intersection")); - p.enable_collision_check_for_prepare_phase_in_turns = - getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); - p.stopped_object_velocity_threshold = - getOrDeclareParameter(*node, parameter("stopped_object_velocity_threshold")); - p.check_objects_on_current_lanes = - getOrDeclareParameter(*node, parameter("check_objects_on_current_lanes")); - p.check_objects_on_other_lanes = - getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); - p.use_all_predicted_path = - getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - p.lane_expansion_left_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); - p.lane_expansion_right_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = @@ -94,136 +115,100 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("regulation.traffic_light")); // ego vehicle stuck detection - p.stop_velocity_threshold = - getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); - p.stop_time_threshold = - getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); - - // safety check - p.allow_loose_check_for_cancel = - getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); - p.collision_check_yaw_diff_threshold = getOrDeclareParameter( - *node, parameter("safety_check.collision_check_yaw_diff_threshold")); - - p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.execution.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.execution.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.lateral_distance_max_threshold")); - - p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); - p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); - p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); - p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.parked.expected_front_deceleration")); - p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.parked.expected_rear_deceleration")); - p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); - p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); - p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.lateral_distance_max_threshold")); - - p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); - p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); - p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.cancel.expected_front_deceleration")); - p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.cancel.expected_rear_deceleration")); - p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); - 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")); + p.th_stop_velocity = getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); + p.th_stop_time = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + + // safety + { + p.safety.enable_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.safety.enable_target_lane_bound_check = + getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); + p.safety.th_stopped_object_velocity = getOrDeclareParameter( + *node, parameter("safety_check.stopped_object_velocity_threshold")); + p.safety.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.safety.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + + // collision check + p.safety.collision_check.enable_for_prepare_phase_in_general_lanes = + getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.general_lanes")); + p.safety.collision_check.enable_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.intersection")); + p.safety.collision_check.enable_for_prepare_phase_in_turns = getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.turns")); + p.safety.collision_check.check_current_lane = + getOrDeclareParameter(*node, parameter("collision_check.check_current_lanes")); + p.safety.collision_check.check_other_lanes = + getOrDeclareParameter(*node, parameter("collision_check.check_other_lanes")); + p.safety.collision_check.use_all_predicted_paths = + getOrDeclareParameter(*node, parameter("collision_check.use_all_predicted_paths")); + p.safety.collision_check.prediction_time_resolution = + getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); + p.safety.collision_check.th_yaw_diff = + getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + + // rss check + auto set_rss_params = [&](auto & params, const std::string & prefix) { + params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter(prefix + ".longitudinal_distance_min_threshold")); + params.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter(prefix + ".longitudinal_velocity_delta_time")); + params.front_vehicle_deceleration = + getOrDeclareParameter(*node, parameter(prefix + ".expected_front_deceleration")); + params.rear_vehicle_deceleration = + getOrDeclareParameter(*node, parameter(prefix + ".expected_rear_deceleration")); + params.rear_vehicle_reaction_time = + getOrDeclareParameter(*node, parameter(prefix + ".rear_vehicle_reaction_time")); + params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); + params.lateral_distance_max_threshold = + getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + }; + set_rss_params(p.safety.rss_params, "safety_check.execution"); + set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); + set_rss_params(p.safety.rss_params_for_abort, "safety_check.cancel"); + set_rss_params(p.safety.rss_params_for_stuck, "safety_check.stuck"); + + // target object types + const std::string ns = "lane_change.target_object."; + p.safety.target_object_types.check_car = getOrDeclareParameter(*node, ns + "car"); + p.safety.target_object_types.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.safety.target_object_types.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.safety.target_object_types.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.safety.target_object_types.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.safety.target_object_types.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.safety.target_object_types.check_motorcycle = + getOrDeclareParameter(*node, ns + "motorcycle"); + p.safety.target_object_types.check_pedestrian = + getOrDeclareParameter(*node, ns + "pedestrian"); + } // lane change parameters - const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); p.backward_length_buffer_for_blocking_object = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); - p.lane_changing_lateral_jerk = - getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); - p.lane_change_prepare_duration = - getOrDeclareParameter(*node, parameter("prepare_duration")); - p.minimum_lane_changing_velocity = - getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); - p.minimum_lane_changing_velocity = - std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); + p.backward_length_from_intersection = + getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "Lane change buffer must be more than 1 meter. Modifying the buffer."); } - // lateral acceleration map for lane change - const auto lateral_acc_velocity = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); - const auto min_lateral_acc = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.min_values")); - const auto max_lateral_acc = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.max_values")); - if ( - lateral_acc_velocity.size() != min_lateral_acc.size() || - lateral_acc_velocity.size() != max_lateral_acc.size()) { - RCLCPP_ERROR( - node->get_logger().get_child(name()), - "Lane change lateral acceleration map has invalid size."); - exit(EXIT_FAILURE); - } - for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { - p.lane_change_lat_acc_map.add( - lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); - } - - // target object - { - const std::string ns = "lane_change.target_object."; - p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); - p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); - p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); - p.object_types_to_check.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); - p.object_types_to_check.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); - p.object_types_to_check.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); - p.object_types_to_check.check_motorcycle = - getOrDeclareParameter(*node, ns + "motorcycle"); - p.object_types_to_check.check_pedestrian = - getOrDeclareParameter(*node, ns + "pedestrian"); - } + // lane change delay + p.delay.enable = getOrDeclareParameter(*node, parameter("delay_lane_change.enable")); + p.delay.check_only_parked_vehicle = + getOrDeclareParameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); + p.delay.min_road_shoulder_width = + getOrDeclareParameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( + *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); // lane change cancel p.cancel.enable_on_prepare_phase = @@ -236,7 +221,7 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); p.cancel.overhang_tolerance = getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); - p.cancel.unsafe_hysteresis_threshold = + p.cancel.th_unsafe_hysteresis = getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); p.cancel.deceleration_sampling_num = getOrDeclareParameter(*node, parameter("cancel.deceleration_sampling_num")); @@ -244,57 +229,54 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // finish judge parameters p.lane_change_finish_judge_buffer = getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); - p.finish_judge_lateral_threshold = + p.th_finish_judge_lateral_diff = getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); const auto finish_judge_lateral_angle_deviation = getOrDeclareParameter(*node, parameter("finish_judge_lateral_angle_deviation")); - p.finish_judge_lateral_angle_deviation = + p.th_finish_judge_yaw_diff = autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation); // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); - // validation of parameters - if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - node->get_logger().get_child(name()), - "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " - << p.longitudinal_acc_sampling_num - << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - // validation of safety check parameters - // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // if loose check is not enabled, lane change module will keep on chattering and canceling, and // false positive situation might occur - if (!p.allow_loose_check_for_cancel) { + if (!p.safety.enable_loose_check_for_cancel) { if ( - p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || - p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || - p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || - p.rss_params.rear_vehicle_safety_time_margin > - p.rss_params_for_abort.rear_vehicle_safety_time_margin || - p.rss_params.lateral_distance_max_threshold > - p.rss_params_for_abort.lateral_distance_max_threshold || - p.rss_params.longitudinal_distance_min_threshold > - p.rss_params_for_abort.longitudinal_distance_min_threshold || - p.rss_params.longitudinal_velocity_delta_time > - p.rss_params_for_abort.longitudinal_velocity_delta_time) { + p.safety.rss_params.front_vehicle_deceleration > + p.safety.rss_params_for_abort.front_vehicle_deceleration || + p.safety.rss_params.rear_vehicle_deceleration > + p.safety.rss_params_for_abort.rear_vehicle_deceleration || + p.safety.rss_params.rear_vehicle_reaction_time > + p.safety.rss_params_for_abort.rear_vehicle_reaction_time || + p.safety.rss_params.rear_vehicle_safety_time_margin > + p.safety.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.safety.rss_params.lateral_distance_max_threshold > + p.safety.rss_params_for_abort.lateral_distance_max_threshold || + p.safety.rss_params.longitudinal_distance_min_threshold > + p.safety.rss_params_for_abort.longitudinal_distance_min_threshold || + p.safety.rss_params.longitudinal_velocity_delta_time > + p.safety.rss_params_for_abort.longitudinal_velocity_delta_time) { RCLCPP_FATAL_STREAM( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "abort parameter might be loose... Terminating the program..."); exit(EXIT_FAILURE); } } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "cancel.delta_time: " << p.cancel.delta_time << ", is too short. This could cause a danger behavior."); } - parameters_ = std::make_shared(p); + return std::make_shared(p); +} + +void LaneChangeModuleManager::initParams(rclcpp::Node * node) +{ + parameters_ = set_params(node, name()); } std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() @@ -314,8 +296,6 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "backward_lane_length", p->backward_lane_length); - updateParam(parameters, ns + "prepare_duration", p->lane_change_prepare_duration); - updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", p->backward_length_buffer_for_end_of_lane); @@ -326,59 +306,62 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorlane_change_finish_judge_buffer); updateParam( - parameters, ns + "lane_changing_lateral_jerk", p->lane_changing_lateral_jerk); + parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + } + { + const std::string ns = "lane_change.trajectory."; updateParam( - parameters, ns + "minimum_lane_changing_velocity", p->minimum_lane_changing_velocity); + parameters, ns + "max_prepare_duration", p->trajectory.max_prepare_duration); updateParam( - parameters, ns + "prediction_time_resolution", p->prediction_time_resolution); - + parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration); + updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); + updateParam( + parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); + // longitudinal acceleration + updateParam( + parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); + updateParam( + parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); + updateParam( + parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); int longitudinal_acc_sampling_num = 0; - updateParam( - parameters, ns + "longitudinal_acceleration_sampling_num", longitudinal_acc_sampling_num); + updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { - p->longitudinal_acc_sampling_num = longitudinal_acc_sampling_num; + p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; } int lateral_acc_sampling_num = 0; - updateParam( - parameters, ns + "lateral_acceleration_sampling_num", lateral_acc_sampling_num); + updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { - p->lateral_acc_sampling_num = lateral_acc_sampling_num; + p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; } updateParam( - parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - - // longitudinal acceleration - updateParam(parameters, ns + "min_longitudinal_acc", p->min_longitudinal_acc); - updateParam(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc); - } - - { - const std::string ns = "lane_change.skip_process.longitudinal_distance_diff_threshold."; - updateParam(parameters, ns + "prepare", p->skip_process_lon_diff_th_prepare); + parameters, ns + "th_prepare_length_diff", p->trajectory.th_prepare_length_diff); updateParam( - parameters, ns + "lane_changing", p->skip_process_lon_diff_th_lane_changing); + parameters, ns + "th_lane_changing_length_diff", p->trajectory.th_lane_changing_length_diff); } { const std::string ns = "lane_change.safety_check.lane_expansion."; - updateParam(parameters, ns + "left_offset", p->lane_expansion_left_offset); - updateParam(parameters, ns + "right_offset", p->lane_expansion_right_offset); + updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); + updateParam(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } { const std::string ns = "lane_change.target_object."; - updateParam(parameters, ns + "car", p->object_types_to_check.check_car); - updateParam(parameters, ns + "truck", p->object_types_to_check.check_truck); - updateParam(parameters, ns + "bus", p->object_types_to_check.check_bus); - updateParam(parameters, ns + "trailer", p->object_types_to_check.check_trailer); - updateParam(parameters, ns + "unknown", p->object_types_to_check.check_unknown); - updateParam(parameters, ns + "bicycle", p->object_types_to_check.check_bicycle); - updateParam(parameters, ns + "motorcycle", p->object_types_to_check.check_motorcycle); - updateParam(parameters, ns + "pedestrian", p->object_types_to_check.check_pedestrian); + updateParam(parameters, ns + "car", p->safety.target_object_types.check_car); + updateParam(parameters, ns + "truck", p->safety.target_object_types.check_truck); + updateParam(parameters, ns + "bus", p->safety.target_object_types.check_bus); + updateParam(parameters, ns + "trailer", p->safety.target_object_types.check_trailer); + updateParam(parameters, ns + "unknown", p->safety.target_object_types.check_unknown); + updateParam(parameters, ns + "bicycle", p->safety.target_object_types.check_bicycle); + updateParam( + parameters, ns + "motorcycle", p->safety.target_object_types.check_motorcycle); + updateParam( + parameters, ns + "pedestrian", p->safety.target_object_types.check_pedestrian); } { @@ -390,8 +373,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "velocity", p->stop_velocity_threshold); - updateParam(parameters, ns + "stop_time", p->stop_time_threshold); + updateParam(parameters, ns + "velocity", p->th_stop_velocity); + updateParam(parameters, ns + "stop_time", p->th_stop_time); } { @@ -416,7 +399,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); updateParam(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); updateParam( - parameters, ns + "unsafe_hysteresis_threshold", p->cancel.unsafe_hysteresis_threshold); + parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 28e95580a4bc7..adbf9ea52364e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -28,6 +28,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -43,9 +47,8 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::create_lanes_polygon; -using utils::path_safety_checker::isPolygonOverlapLanelet; -using utils::traffic_light::getDistanceToNextTrafficLight; namespace calculation = utils::lane_change::calculation; +using utils::path_safety_checker::filter::velocity_filter; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -69,11 +72,18 @@ void NormalLaneChange::update_lanes(const bool is_approved) return; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + const auto target_lanes = get_lane_change_lanes(current_lanes); if (target_lanes.empty()) { return; } + lanelet::ConstLanelet current_lane; + if (!common_data_ptr_->route_handler_ptr->getClosestLaneletWithinRoute( + common_data_ptr_->get_ego_pose(), ¤t_lane)) { + return; + } + + common_data_ptr_->lanes_ptr->ego_lane = current_lane; const auto is_same_lanes_with_prev_iteration = utils::lane_change::is_same_lane_with_prev_iteration( common_data_ptr_, current_lanes, target_lanes); @@ -86,35 +96,136 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target = target_lanes; const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( + common_data_ptr_->current_lanes_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); + common_data_ptr_->current_lanes_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(current_lanes.back()); - common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( - *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), - common_data_ptr_->lc_param_ptr->backward_lane_length); + common_data_ptr_->lanes_ptr->target_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(target_lanes.back()); + + common_data_ptr_->lanes_ptr->preceding_target = + utils::lane_change::get_preceding_lanes(common_data_ptr_); + + lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current; + lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target; + + lane_change_debug_.target_backward_lanes.clear(); + ranges::for_each( + common_data_ptr_->lanes_ptr->preceding_target, + [&](const lanelet::ConstLanelets & preceding_lanes) { + ranges::insert( + lane_change_debug_.target_backward_lanes, lane_change_debug_.target_backward_lanes.end(), + preceding_lanes); + }); *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } +void NormalLaneChange::update_transient_data(const bool is_approved) +{ + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { + return; + } + + auto & transient_data = common_data_ptr_->transient_data; + + const auto & p = *common_data_ptr_->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_output_.path.points, common_data_ptr_->get_ego_pose(), + p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); + transient_data.current_path_velocity = + prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps; + transient_data.current_path_seg_idx = nearest_seg_idx; + + const auto active_signal_duration = + signal_activation_time_ ? (clock_.now() - signal_activation_time_.value()).seconds() : 0.0; + transient_data.lane_change_prepare_duration = + is_approved ? status_.lane_change_path.info.duration.prepare + : calculation::calc_actual_prepare_duration( + common_data_ptr_, common_data_ptr_->get_ego_speed(), active_signal_duration); + + std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = + calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); + + transient_data.next_dist_buffer.min = + transient_data.current_dist_buffer.min - transient_data.lane_changing_length.min - + common_data_ptr_->lc_param_ptr->lane_change_finish_judge_buffer; + + transient_data.dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + transient_data.dist_to_terminal_start = + transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + + transient_data.target_lane_length = + lanelet::utils::getLaneletLength2d(common_data_ptr_->lanes_ptr->target); + + transient_data.current_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + + transient_data.target_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + + transient_data.is_ego_near_current_terminal_start = + transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + + transient_data.current_footprint = utils::lane_change::get_ego_footprint( + common_data_ptr_->get_ego_pose(), common_data_ptr_->bpp_param_ptr->vehicle_info); + + const auto & ego_lane = common_data_ptr_->lanes_ptr->ego_lane; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + transient_data.in_intersection = utils::lane_change::is_within_intersection( + route_handler_ptr, ego_lane, transient_data.current_footprint); + transient_data.in_turn_direction_lane = + utils::lane_change::is_within_turn_direction_lanes(ego_lane, transient_data.current_footprint); + + update_dist_from_intersection(); + + updateStopTime(); + transient_data.is_ego_stuck = is_ego_stuck(); + + RCLCPP_DEBUG( + logger_, "lane_changing_length - min: %.4f, max: %.4f", transient_data.lane_changing_length.min, + transient_data.lane_changing_length.max); + RCLCPP_DEBUG( + logger_, "current_dist_buffer - min: %.4f, max: %.4f", transient_data.current_dist_buffer.min, + transient_data.current_dist_buffer.max); + RCLCPP_DEBUG( + logger_, "next_dist_buffer - min: %.4f, max: %.4f", transient_data.next_dist_buffer.min, + transient_data.next_dist_buffer.max); + RCLCPP_DEBUG(logger_, "dist_to_terminal_start: %.4f", transient_data.dist_to_terminal_start); + RCLCPP_DEBUG(logger_, "dist_to_terminal_end: %.4f", transient_data.dist_to_terminal_end); + RCLCPP_DEBUG(logger_, "max_prepare_length: %.4f", transient_data.max_prepare_length); + RCLCPP_DEBUG( + logger_, "is_ego_near_current_terminal_start: %s", + (transient_data.is_ego_near_current_terminal_start ? "true" : "false")); +} + void NormalLaneChange::update_filtered_objects() { - filtered_objects_ = filterObjects(); + filtered_objects_ = filter_objects(); } void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); - status_.start_distance = arclength_start.length; + status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -128,9 +239,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) } LaneChangePaths valid_paths{}; - const bool is_stuck = isVehicleStuck(current_lanes); - bool found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, is_stuck, &valid_paths); + bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin lane_change_debug_.valid_paths = valid_paths; @@ -153,24 +262,20 @@ bool NormalLaneChange::isLaneChangeRequired() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto current_lanes = - utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - - if (current_lanes.empty()) { + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { - return false; - } + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto ego_dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - if (ego_dist_to_target_start > maximum_prepare_length) { + if (ego_dist_to_target_start > max_prepare_length) { return false; } @@ -184,32 +289,19 @@ bool NormalLaneChange::isLaneChangeRequired() bool NormalLaneChange::is_near_regulatory_element() const { - const auto & current_lanes = get_current_lanes(); + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - if (current_lanes.empty()) return false; + if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back()); - - if (shift_intervals.empty()) return false; - - const auto & lc_params = *common_data_ptr_->lc_param_ptr; - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto min_lc_length = - calculation::calc_minimum_lane_change_length(lc_params, shift_intervals); - const auto dist_to_terminal_start = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_) - min_lc_length; - - if (dist_to_terminal_start <= max_prepare_length) return false; - - const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; if (only_tl) { RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); } - return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( - common_data_ptr_, only_tl, only_tl); + return common_data_ptr_->transient_data.max_prepare_length > + utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -239,6 +331,8 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const return get_terminal_turn_signal_info(); } + set_signal_activation_time(); + return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end); } @@ -251,14 +345,9 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); - const double next_lane_change_buffer = - calculation::calc_minimum_lane_change_length(lane_change_param, shift_intervals); - - const double buffer = next_lane_change_buffer + - lane_change_param.min_length_for_turn_signal_activation + - common_param.base_link2front; + const auto buffer = common_data_ptr_->transient_data.current_dist_buffer.min + + lane_change_param.min_length_for_turn_signal_activation + + common_param.base_link2front; const double path_length = autoware::motion_utils::calcArcLength(path.points); const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, 0, std::max(path_length - buffer, 0.0)); @@ -268,15 +357,18 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto terminal_turn_signal_info = get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end); - const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; - const size_t current_nearest_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const auto nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx; - return getTurnSignalDecider().overwrite_turn_signal( + const auto turn_signal_info = getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); + + set_signal_activation_time( + turn_signal_info.turn_signal.command != terminal_turn_signal_info.turn_signal.command); + + return turn_signal_info; } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -306,7 +398,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const } const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); if (!terminal_path) { RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); return prev_module_output_; @@ -331,14 +423,14 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); - insertStopPoint(get_current_lanes(), prev_module_output_.path); + insert_stop_point(get_current_lanes(), prev_module_output_.path); return prev_module_output_; } auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(get_current_lanes(), output.path); + insert_stop_point(get_current_lanes(), output.path); } else { output.path = status_.lane_change_path.path; @@ -347,8 +439,6 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path = utils::combinePath(output.path, *found_extended_path); } output.reference_path = getReferencePath(); - output.turn_signal_info = - get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); if (isStopState()) { const auto current_velocity = getEgoVelocity(); @@ -356,21 +446,25 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() 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)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); - setStopPose(stop_point.point.pose); + set_stop_pose(stop_dist + current_dist, output.path); } else { - insertStopPoint(get_target_lanes(), output.path); + insert_stop_point(get_target_lanes(), output.path); } } extendOutputDrivableArea(output); + const auto turn_signal_info = + get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, + turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); + set_signal_activation_time( + output.turn_signal_info.turn_signal.command != turn_signal_info.turn_signal.command); + return output; } @@ -393,7 +487,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c current_drivable_area_info, prev_module_output_.drivable_area_info); } -void NormalLaneChange::insertStopPoint( +void NormalLaneChange::insert_stop_point( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -407,126 +501,110 @@ void NormalLaneChange::insertStopPoint( return; } - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const auto lane_change_buffer = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); + const auto & current_lanes = get_current_lanes(); + const auto is_current_lane = lanelets.front().id() == current_lanes.front().id() && + lanelets.back().id() == current_lanes.back().id(); + + // if input is not current lane, we can just insert the points at terminal. + if (!is_current_lane) { + const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - + common_data_ptr_->transient_data.next_dist_buffer.min; + set_stop_pose(arc_length_to_stop_pose, path); + return; + } + + insert_stop_point_on_current_lanes(path); +} - const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { - return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); +void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +{ + const auto & path_front_pose = path.points.front().point.pose; + const auto & center_line = common_data_ptr_->current_lanes_path.points; + const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) { + return motion_utils::calcSignedArcLength( + center_line, path_front_pose.position, target.position); }; - // 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 = getDistanceAlongLanelet(goal); - } else { - distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); - } + const auto & transient_data = common_data_ptr_->transient_data; + const auto & lanes_ptr = common_data_ptr_->lanes_ptr; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(); - double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto dist_to_terminal = std::invoke([&]() -> double { + const auto target_pose = (lanes_ptr->current_lane_in_goal_section) + ? common_data_ptr_->route_handler_ptr->getGoalPose() + : center_line.back().point.pose; + return get_arc_length_along_lanelet(target_pose); + }); - const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; + const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; + const auto min_dist_buffer = transient_data.current_dist_buffer.min; + const auto dist_to_terminal_start = + dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); + + const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; + if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { + return utils::lane_change::calculation::calc_dist_to_last_fit_width( + lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + } + return std::numeric_limits::max(); + }); - if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); + const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + if (filtered_objects_.current_lane.empty()) { + set_stop_pose(dist_to_terminal_stop, path); return; } - // 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.linear.x); - if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { - continue; - } + const auto dist_to_target_lane_start = std::invoke([&]() -> double { + const auto & front_lane = lanes_ptr->target_neighbor.front(); + const auto target_front = + utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane); + return get_arc_length_along_lanelet(target_front); + }); - // calculate distance from path front to the stationary object polygon on the ego lane. - const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); - for (const auto & polygon_p : polygon) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); + const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( + common_data_ptr_, filtered_objects_, dist_to_target_lane_start, path); - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } + // margin with leading vehicle + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, lc_param_ptr->trajectory.min_lane_changing_velocity, + lc_param_ptr->safety.rss_params_for_parked); - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + const auto stop_margin = transient_data.lane_changing_length.min + + lc_param_ptr->backward_length_buffer_for_blocking_object + rss_dist + + bpp_param_ptr->base_link2front; + const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; - // 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; - }(); - - // 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, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); - const double lane_change_buffer_for_blocking_object = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); - - const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - - lane_change_parameters_->backward_length_buffer_for_blocking_object - 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_leading.begin(), target_objects.target_lane_leading.end(), - [&](const auto & o) { - const auto v = std::abs(o.initial_twist.linear.x); - if (v > lane_change_parameters_->stopped_object_velocity_threshold) { - return false; - } + if (stop_arc_length_behind_obj < dist_to_target_lane_start) { + set_stop_pose(dist_to_target_lane_start, path); + return; + } - // target_objects includes objects out of target lanes, so filter them out - if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(get_target_lanes()) - .polygon2d() - .basicPolygon())) { - return false; - } + if (stop_arc_length_behind_obj > dist_to_terminal_stop) { + set_stop_pose(dist_to_terminal_stop, path); + return; + } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); - return stopping_distance_for_obj < distance_to_target_lane_obj && - distance_to_target_lane_obj < distance_to_ego_lane_obj; - }); + // 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]> | <--- stop margin ---> [obj]> + // ---------------------------------------------------------- + const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object( + filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path); - if (!has_blocking_target_lane_obj) { - stopping_distance = stopping_distance_for_obj; - } + if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { + set_stop_pose(dist_to_terminal_stop, path); + return; } - if (stopping_distance > 0.0) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); - } + set_stop_pose(stop_arc_length_behind_obj, path); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -537,7 +615,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const get_target_lanes(), getEgoPose(), &closest_lanelet)) { return prev_module_output_.reference_path; } - const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); + auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); if (reference_path.points.empty()) { return prev_module_output_.reference_path; } @@ -548,39 +626,32 @@ std::optional NormalLaneChange::extendPath() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); 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 = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); - - if (dist < 0.0) { - return std::nullopt; - } auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); + const auto & transient_data = common_data_ptr_->transient_data; const auto forward_path_length = getCommonParam().forward_path_length; - if ((target_lane_length - dist_in_target.length) > forward_path_length) { + if ( + (transient_data.target_lane_length - transient_data.target_lanes_ego_arc.length) > + forward_path_length) { return std::nullopt; } + const auto dist_to_end_of_path = + lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; - const auto is_goal_in_target = getRouteHandler()->isInGoalRouteSection(target_lanes.back()); - - if (is_goal_in_target) { + if (common_data_ptr_->lanes_ptr->target_lane_in_goal_section) { const auto goal_pose = getRouteHandler()->getGoalPose(); const auto dist_to_goal = lanelet::utils::getArcCoordinates(target_lanes, goal_pose).length; - const auto dist_to_end_of_path = - lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; return getRouteHandler()->getCenterLinePath(target_lanes, dist_to_end_of_path, dist_to_goal); } lanelet::ConstLanelet next_lane; if (!getRouteHandler()->getNextLaneletWithinRoute(target_lanes.back(), &next_lane)) { - return std::nullopt; + return getRouteHandler()->getCenterLinePath( + target_lanes, dist_to_end_of_path, transient_data.target_lane_length); } target_lanes.push_back(next_lane); @@ -591,32 +662,23 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getGoalPose(); } - Pose back_pose; - const auto back_point = - lanelet::utils::conversion::toGeomMsgPt(next_lane.centerline2d().back()); - const double front_yaw = lanelet::utils::getLaneletAngle(next_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - return back_pose; + return utils::to_geom_msg_pose(next_lane.centerline2d().back(), next_lane); }); const auto dist_to_target_pose = lanelet::utils::getArcCoordinates(target_lanes, target_pose).length; - const auto dist_to_end_of_path = - lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose); } + void NormalLaneChange::resetParameters() { is_abort_path_approved_ = false; is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; - status_ = {}; + status_ = LaneChangeStatus(); unsafe_hysteresis_count_ = 0; lane_change_debug_.reset(); @@ -626,11 +688,10 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & pose = getEgoPose(); const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; - const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + const auto current_shift_length = common_data_ptr_->transient_data.current_lanes_ego_arc.distance; constexpr bool is_driving_forward = true; // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. @@ -644,8 +705,8 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const +lanelet::ConstLanelets NormalLaneChange::get_lane_change_lanes( + const lanelet::ConstLanelets & current_lanes) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { @@ -654,69 +715,27 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( // Get lane change lanes const auto & route_handler = getRouteHandler(); - const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( - *getRouteHandler(), current_lanes, type_, direction); + const auto lane_change_lane = + utils::lane_change::get_lane_change_target_lane(common_data_ptr_, current_lanes); if (!lane_change_lane) { return {}; } - const auto front_pose = std::invoke([&lane_change_lane]() { - const auto & p = lane_change_lane->centerline().front(); - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); - const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); - geometry_msgs::msg::Pose front_pose; - front_pose.position = front_point; - tf2::Quaternion quat; - quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(quat); - return front_pose; - }); - const auto forward_length = std::invoke([&]() { + const auto front_pose = + utils::to_geom_msg_pose(lane_change_lane->centerline().front(), *lane_change_lane); const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); const auto forward_path_length = planner_data_->parameters.forward_path_length; - if (signed_distance <= 0.0) { - return forward_path_length; - } - - return signed_distance + forward_path_length; + return forward_path_length + std::max(signed_distance, 0.0); }); + const auto backward_length = lane_change_parameters_->backward_lane_length; return route_handler->getLaneletSequence( lane_change_lane.value(), getEgoPose(), backward_length, forward_length); } -bool NormalLaneChange::isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const -{ - if (current_lanes.empty()) { - return false; - } - - const auto & route_handler = getRouteHandler(); - const auto & current_pose = getEgoPose(); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); - - const auto distance_to_lane_change_end = std::invoke([&]() { - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } - - return std::max(0.0, distance_to_end) - lane_change_buffer; - }); - - lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; - return distance_to_lane_change_end < threshold; -} - bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -742,20 +761,20 @@ bool NormalLaneChange::hasFinishedLaneChange() const if (has_passed_end_pose) { const auto & lanes_polygon = common_data_ptr_->lanes_polygon_ptr->target; return !boost::geometry::disjoint( - lanes_polygon.value(), + lanes_polygon, lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(current_pose.position))); } const auto yaw_deviation_to_centerline = utils::lane_change::calc_angle_to_lanelet_segment(target_lanes, current_pose); - if (yaw_deviation_to_centerline > lane_change_parameters_->finish_judge_lateral_angle_deviation) { + if (yaw_deviation_to_centerline > lane_change_parameters_->th_finish_judge_yaw_diff) { return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = - std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; + std::abs(arc_length.distance) < lane_change_parameters_->th_finish_judge_lateral_diff; lane_change_debug_.distance_to_lane_change_finished = arc_length.distance; @@ -769,20 +788,25 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; if (!utils::isEgoWithinOriginalLane( - get_current_lanes(), getEgoPose(), planner_data_->parameters, + curr_lanes_poly, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; } + if (common_data_ptr_->transient_data.in_turn_direction_lane) { + return true; + } + const auto nearest_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); const double ego_velocity = - std::max(getEgoVelocity(), lane_change_parameters_->minimum_lane_changing_velocity); + std::max(getEgoVelocity(), lane_change_parameters_->trajectory.min_lane_changing_velocity); const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; double dist = 0.0; @@ -791,7 +815,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters, + curr_lanes_poly, estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -804,27 +828,18 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const bool NormalLaneChange::is_near_terminal() const { - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - - if (current_lanes.empty()) { + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) { return true; } - const auto & current_lanes_terminal = current_lanes.back(); + // TODO(Azu) fully change to transient data const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto direction = common_data_ptr_->direction; - const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - const auto min_lane_changing_distance = calculation::calc_minimum_lane_change_length( - route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); - const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr); - const auto min_lc_dist_with_buffer = - backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; - const auto dist_from_ego_to_terminal_end = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto min_lc_dist_with_buffer = backward_buffer + current_min_dist_buffer; - return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer; + return common_data_ptr_->transient_data.dist_to_terminal_end < min_lc_dist_with_buffer; } bool NormalLaneChange::isEgoOnPreparePhase() const @@ -849,13 +864,14 @@ bool NormalLaneChange::isAbleToStopSafely() const const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { 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( - get_current_lanes(), estimated_pose, planner_data_->parameters); + curr_lanes_poly, estimated_pose, planner_data_->parameters); } } return true; @@ -895,6 +911,7 @@ bool NormalLaneChange::isAbortState() const lane_change_debug_.is_abort = true; return true; } + int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const { const auto get_opposite_direction = @@ -902,157 +919,75 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } -std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() const +bool NormalLaneChange::get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) 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 = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_module_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold); - const auto max_path_velocity = - prev_module_output_.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, *lane_change_parameters_); - const auto max_acc = utils::lane_change::calcMaximumAcceleration( - getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_); - - return {min_acc, max_acc}; -} - -std::vector NormalLaneChange::sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const -{ - if (prev_module_output_.path.points.empty()) { - return {}; - } - - const auto & route_handler = *getRouteHandler(); - const auto current_pose = getEgoPose(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; - // 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); + if (current_lanes.empty() || target_lanes.empty()) { + return false; } - // calculate maximum lane change length - const double max_lane_change_length = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); + prepare_segment = prev_module_output_.path; + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, getEgoPose(), 3.0, 1.0); + utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - 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); - } - - // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuck(current_lanes)) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_INFO_THROTTLE( - 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); - } - - // 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())) { - 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}; - } + if (prepare_segment.points.empty()) return false; - 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); -} + const auto & lc_start_pose = prepare_segment.points.back().point.pose; -std::vector NormalLaneChange::calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const -{ - const auto base_link2front = planner_data_->parameters.base_link2front; - const auto threshold = - lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; - std::vector prepare_durations; - constexpr double step = 0.5; + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); - for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; - duration -= step) { - prepare_durations.push_back(duration); - if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { - break; - } + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); } - return prepare_durations; + return true; } -PathWithLaneId NormalLaneChange::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const +lane_change::TargetObjects NormalLaneChange::get_target_objects( + const FilteredLanesObjects & filtered_objects, + [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { - if (current_lanes.empty()) { - return PathWithLaneId(); - } - - auto prepare_segment = prev_module_output_.path; - const size_t current_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, getEgoPose(), 3.0, 1.0); - utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - - return prepare_segment; -} + ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading.moving; + auto insert_leading_objects = [&](const auto & objects) { + ranges::actions::insert(leading_objects, leading_objects.end(), objects); + }; -lane_change::TargetObjects NormalLaneChange::getTargetObjects( - const FilteredByLanesExtendedObjects & filtered_objects, - const lanelet::ConstLanelets & current_lanes) const -{ - ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; - const auto is_stuck = isVehicleStuck(current_lanes); - const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; - if (chk_obj_in_curr_lanes || is_stuck) { - leading_objects.insert( - leading_objects.end(), filtered_objects.current_lane.begin(), - filtered_objects.current_lane.end()); + insert_leading_objects(filtered_objects.target_lane_leading.stopped); + insert_leading_objects(filtered_objects.target_lane_leading.stopped_at_bound); + const auto chk_obj_in_curr_lanes = + lane_change_parameters_->safety.collision_check.check_current_lane; + if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { + insert_leading_objects(filtered_objects.current_lane); } - const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; + const auto chk_obj_in_other_lanes = + lane_change_parameters_->safety.collision_check.check_other_lanes; if (chk_obj_in_other_lanes) { - leading_objects.insert( - leading_objects.end(), filtered_objects.other_lane.begin(), - filtered_objects.other_lane.end()); + insert_leading_objects(filtered_objects.others); } return {leading_objects, filtered_objects.target_lane_trailing}; } -FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const +FilteredLanesObjects NormalLaneChange::filter_objects() const { - const auto & route_handler = getRouteHandler(); + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( - objects, lane_change_parameters_->object_types_to_check); + objects, lane_change_parameters_->safety.target_object_types); if (objects.objects.empty()) { return {}; @@ -1064,64 +999,81 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto & current_lanes = get_current_lanes(); - - if (current_lanes.empty()) { + if (!common_data_ptr_->is_lanes_available()) { return {}; } - const auto & target_lanes = get_target_lanes(); + const auto & current_pose = common_data_ptr_->get_ego_pose(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - if (target_lanes.empty()) { - return {}; - } + const auto & current_lanes_ref_path = common_data_ptr_->current_lanes_path; - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); + FilteredLanesObjects filtered_objects; + const auto reserve_size = objects.objects.size(); + filtered_objects.current_lane.reserve(reserve_size); + auto & target_lane_leading = filtered_objects.target_lane_leading; + target_lane_leading.stopped.reserve(reserve_size); + target_lane_leading.moving.reserve(reserve_size); + target_lane_leading.stopped_at_bound.reserve(reserve_size); + filtered_objects.target_lane_trailing.reserve(reserve_size); + filtered_objects.others.reserve(reserve_size); - const auto is_within_vel_th = [](const auto & object) -> bool { - constexpr double min_vel_th = 1.0; - constexpr double max_vel_th = std::numeric_limits::max(); - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); - }; + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.target_lane_trailing, - [&](const PredictedObject & object) { return is_within_vel_th(object); }); + for (const auto & object : objects.objects) { + auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); + const auto & ext_obj_pose = ext_object.initial_pose; + ext_object.dist_from_ego = autoware::motion_utils::calcSignedArcLength( + current_lanes_ref_path.points, current_pose.position, ext_obj_pose.position); - if (lane_change_parameters_->check_objects_on_other_lanes) { - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.other_lane, [&](const PredictedObject & object) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); - return is_within_vel_th(object) && ahead_of_ego; - }); + const auto is_before_terminal = + utils::lane_change::is_before_terminal(common_data_ptr_, current_lanes_ref_path, ext_object); + + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, ext_object); + + if (utils::lane_change::filter_target_lane_objects( + common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego, + is_before_terminal, target_lane_leading, filtered_objects.target_lane_trailing)) { + continue; + } + + // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior + const auto is_moving = velocity_filter( + ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); + + if ( + ahead_of_ego && is_moving && is_before_terminal && + !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { + // check only the objects that are in front of the ego vehicle + filtered_objects.current_lane.push_back(ext_object); + continue; + } + + filtered_objects.others.push_back(ext_object); } - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.current_lane, [&](const PredictedObject & object) { - const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); - return is_within_vel_th(object) && ahead_of_ego; - }); + const auto dist_comparator = [](const auto & obj1, const auto & obj2) { + return obj1.dist_from_ego < obj2.dist_from_ego; + }; + + // There are no use cases for other lane objects yet, so to save some computation time, we dont + // have to sort them. + ranges::sort(filtered_objects.current_lane, dist_comparator); + ranges::sort(target_lane_leading.stopped_at_bound, dist_comparator); + ranges::sort(target_lane_leading.stopped, dist_comparator); + ranges::sort(target_lane_leading.moving, dist_comparator); + ranges::sort(filtered_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { + return !dist_comparator(obj1, obj2); + }); - const auto target_lane_leading_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_leading); - const auto target_lane_trailing_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing); - const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.current_lane); - const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.other_lane); - - FilteredByLanesExtendedObjects lane_change_target_objects( - current_lane_extended_objects, target_lane_leading_extended_objects, - target_lane_trailing_extended_objects, other_lane_extended_objects); - lane_change_debug_.filtered_objects = lane_change_target_objects; - return lane_change_target_objects; + lane_change_debug_.filtered_objects = filtered_objects; + + return filtered_objects; } void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const @@ -1138,7 +1090,8 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_stopped_object = [](const auto & object) -> bool { constexpr double min_vel_th = -0.5; constexpr double max_vel_th = 0.5; - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); + return velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, min_vel_th, max_vel_th); }; utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { @@ -1151,122 +1104,6 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const -{ - std::vector target_lane_leading_objects; - std::vector target_lane_trailing_objects; - std::vector current_lane_objects; - std::vector other_lane_objects; - - const auto & current_pose = getEgoPose(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & route_handler = getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto check_optional_polygon = [](const auto & object, const auto & polygon) { - return polygon && isPolygonOverlapLanelet(object, *polygon); - }; - - // get backward lanes - const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; - - { - lane_change_debug_.current_lanes = current_lanes; - lane_change_debug_.target_lanes = target_lanes; - - // TODO(Azu) change the type to std::vector - lane_change_debug_.target_backward_lanes.clear(); - std::for_each( - target_backward_lanes.begin(), target_backward_lanes.end(), - [&](const lanelet::ConstLanelets & target_backward_lane) { - lane_change_debug_.target_backward_lanes.insert( - lane_change_debug_.target_backward_lanes.end(), target_backward_lane.begin(), - target_backward_lane.end()); - }); - } - - const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; - const auto dist_ego_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - - const auto reserve_size = objects.objects.size(); - current_lane_objects.reserve(reserve_size); - target_lane_leading_objects.reserve(reserve_size); - target_lane_trailing_objects.reserve(reserve_size); - other_lane_objects.reserve(reserve_size); - - for (const auto & object : objects.objects) { - const auto is_lateral_far = std::invoke([&]() -> bool { - const auto dist_object_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet( - current_lanes, object.kinematics.initial_pose_with_covariance.pose); - const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; - return std::abs(lateral) > (common_parameters.vehicle_width / 2); - }); - - const auto is_before_terminal = [&]() { - return utils::lane_change::is_before_terminal( - common_data_ptr_, current_lanes_ref_path, object); - }; - - if ( - check_optional_polygon(object, lanes_polygon.target) && is_lateral_far && - is_before_terminal()) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - if (ahead_of_ego) { - target_lane_leading_objects.push_back(object); - } else { - target_lane_trailing_objects.push_back(object); - } - continue; - } - - if ( - check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && - is_before_terminal()) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - const auto stopped_obj_vel_th = - common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < stopped_obj_vel_th) { - if (ahead_of_ego) { - target_lane_leading_objects.push_back(object); - continue; - } - } - } - - const auto is_overlap_target_backward = std::invoke([&]() -> bool { - const auto check_backward_polygon = [&object](const auto & target_backward_polygon) { - return isPolygonOverlapLanelet(object, target_backward_polygon); - }; - return std::any_of( - lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), - check_backward_polygon); - }); - - // check if the object intersects with target backward lanes - if (is_overlap_target_backward) { - target_lane_trailing_objects.push_back(object); - continue; - } - - if (check_optional_polygon(object, lanes_polygon.current)) { - // check only the objects that are in front of the ego vehicle - current_lane_objects.push_back(object); - continue; - } - - other_lane_objects.push_back(object); - } - - return { - current_lane_objects, target_lane_leading_objects, target_lane_trailing_objects, - other_lane_objects}; -} - PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -1302,443 +1139,284 @@ PathWithLaneId NormalLaneChange::getTargetSegment( return target_segment; } -bool NormalLaneChange::hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction) const +std::vector NormalLaneChange::get_prepare_metrics() const { - if (target_lanes.empty()) { - return false; - } - - const auto current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); - const auto minimum_lane_change_length_to_preferred_lane = - calculation::calc_minimum_lane_change_length( - route_handler, target_lanes.back(), *lane_change_parameters_, direction); - - const double lane_change_length = path.info.length.sum(); - if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - const auto goal_pose = route_handler->getGoalPose(); - if ( - route_handler->isInGoalRouteSection(current_lanes.back()) && - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { - return false; - } - - // return if there are no target lanes - if ( - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - return false; - } + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto current_velocity = getEgoVelocity(); + // set speed limit to be current path velocity; + const auto max_path_velocity = common_data_ptr_->transient_data.current_path_velocity; - return true; + const auto dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + return calculation::calc_prepare_phase_metrics( + common_data_ptr_, current_velocity, max_path_velocity, dist_to_target_start, + common_data_ptr_->transient_data.dist_to_terminal_start); } -bool NormalLaneChange::hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +std::vector NormalLaneChange::get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, + const double shift_length, const double dist_to_reg_element) const { - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - - const double dist_to_crosswalk_from_lane_change_start_pose = - utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - - path.info.length.prepare; - // Check lane changing section includes crosswalk - if ( - dist_to_crosswalk_from_lane_change_start_pose > 0.0 && - dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { - return false; - } + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prep_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + transient_data.is_ego_near_current_terminal_start + ? transient_data.dist_to_terminal_end - prep_metric.length + : std::min(transient_data.dist_to_terminal_end, dist_to_reg_element) - prep_metric.length; + max_length = + std::min(max_length, dist_lc_start_to_end_of_lanes - transient_data.next_dist_buffer.min); + return max_length; + }); - return true; + const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; + return calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); } -bool NormalLaneChange::hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + lane_change_debug_.collision_check_objects.clear(); - const double dist_to_intersection_from_lane_change_start_pose = - utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; - // Check lane changing section includes intersection - if ( - dist_to_intersection_from_lane_change_start_pose > 0.0 && - dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + if (!common_data_ptr_->is_lanes_available()) { + RCLCPP_WARN(logger_, "lanes are not available. Not expected."); return false; } - return true; -} - -bool NormalLaneChange::hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const -{ - const auto current_pose = getEgoPose(); - const auto dist_to_next_traffic_light = - getDistanceToNextTrafficLight(current_pose, current_lanes); - const auto dist_to_next_traffic_light_from_lc_start_pose = - dist_to_next_traffic_light - path.info.length.prepare; - - return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || - dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; -} - -bool NormalLaneChange::getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const -{ - lane_change_debug_.collision_check_objects.clear(); - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + if (common_data_ptr_->lanes_polygon_ptr->target_neighbor.empty()) { + RCLCPP_WARN(logger_, "target_lane_neighbors_polygon_2d is empty. Not expected."); return false; } - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto backward_path_length = common_parameters.backward_path_length; - const auto forward_path_length = common_parameters.forward_path_length; - const auto minimum_lane_changing_velocity = - lane_change_parameters_->minimum_lane_changing_velocity; - const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - // get velocity const auto current_velocity = getEgoVelocity(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); + const auto target_objects = get_target_objects(filtered_objects_, current_lanes); - // get sampling acceleration values - const auto longitudinal_acc_sampling_values = - sampleLongitudinalAccValues(current_lanes, target_lanes); + const auto prepare_phase_metrics = get_prepare_metrics(); - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + candidate_paths.reserve( + prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; + const auto dist_to_next_regulatory_element = + utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - const auto dist_to_end_of_current_lanes = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + auto check_length_diff = + [&](const double prep_length, const double lc_length, const bool check_lc) { + if (candidate_paths.empty()) return true; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); + if (prep_diff > lane_change_parameters_->trajectory.th_prepare_length_diff) return true; - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + if (!check_lc) return false; - const auto & target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return false; - } - - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); - - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); + return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; + }; - candidate_paths->reserve( - longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + for (const auto & prep_metric : prepare_phase_metrics) { + const auto debug_print = [&](const std::string & s) { + RCLCPP_DEBUG( + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), + prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); + }; - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + if (!check_length_diff(prep_metric.length, 0.0, false)) { + RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); + continue; + } - const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; - const auto dist_to_next_regulatory_element = - utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + PathWithLaneId prepare_segment; + try { + if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + debug_print("Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + debug_print(e.what()); + break; + } - for (const auto & prepare_duration : prepare_durations) { - for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - // get path on original lanes - const auto prepare_velocity = std::clamp( - current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity, getCommonParam().max_vel); + debug_print("Prepare path satisfy constraints"); - // compute actual longitudinal acceleration - const double longitudinal_acc_on_prepare = - (prepare_duration < 1e-3) ? 0.0 - : ((prepare_velocity - current_velocity) / prepare_duration); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto prepare_length = utils::lane_change::calcPhaseLength( - current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - const auto ego_dist_to_terminal_start = dist_to_end_of_current_lanes - lane_change_buffer; - if (prepare_length > ego_dist_to_terminal_start) { - RCLCPP_DEBUG( - logger_, - "Reject: Prepare length exceed distance to terminal start. prep_len: %.5f, ego dist to " - "terminal start: %.5f", - prepare_length, ego_dist_to_terminal_start); - continue; - } + const auto lane_changing_metrics = get_lane_changing_metrics( + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - if (std::abs(prev_prep_diff) < lane_change_parameters_->skip_process_lon_diff_th_prepare) { - RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); - continue; - } - } - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); - const auto debug_print = [&](const auto & s) { + for (const auto & lc_metric : lane_changing_metrics) { + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | prep_len: %.5f", s, - prepare_duration, sampled_longitudinal_acc, longitudinal_acc_on_prepare, prepare_length); + logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), + lc_metric.duration, lc_metric.actual_lon_accel, lc_metric.lat_accel, lc_metric.length); }; - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); + if (!check_length_diff(prep_metric.length, lc_metric.length, true)) { + RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); continue; } - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); + LaneChangePath candidate_path; + try { + candidate_path = get_candidate_path( + prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, + shift_length); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); continue; } - // lane changing start getEgoPose() is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); + candidate_paths.push_back(candidate_path); - // 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) { - debug_print("lane change start getEgoPose() is behind target lanelet!"); - break; - } - - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - - const auto initial_lane_changing_velocity = prepare_velocity; - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - - // get lateral acceleration range - const auto [min_lateral_acc, max_lateral_acc] = - lane_change_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 eps = 0.01; - for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) { - sample_lat_acc.push_back(a); - } - RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); - - debug_print("Prepare path satisfy constraints"); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - for (const auto & lateral_acc : sample_lat_acc) { - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); - const double longitudinal_acc_on_lane_changing = - utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, - sampled_longitudinal_acc); - const auto lane_changing_length = utils::lane_change::calcPhaseLength( - initial_lane_changing_velocity, getCommonParam().max_vel, - longitudinal_acc_on_lane_changing, lane_changing_time); - - const auto debug_print_lat = [&](const auto & s) { - RCLCPP_DEBUG( - logger_, - " - %s | lc_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | lc_len: %.5f", s, - lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing, - lane_changing_length); - }; - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - const auto lc_length_diff = - candidate_paths->back().info.length.lane_changing - lane_changing_length; - - // We only check lc_length_diff if and only if the current prepare_length is equal to the - // previous prepare_length. - if ( - std::abs(prev_prep_diff) < eps && - std::abs(lc_length_diff) < - lane_change_parameters_->skip_process_lon_diff_th_lane_changing) { - RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); - continue; - } - } - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; + try { + if (check_candidate_path_safety(candidate_path, target_objects)) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; } + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); + return false; + } - if ( - ego_dist_to_terminal_start > max_prepare_length && - lane_changing_length + prepare_length > dist_to_next_regulatory_element) { - debug_print_lat( - "Reject: length of lane changing path is longer than length to regulatory element!!"); - continue; - } + debug_print_lat("Reject: sampled path is not safe."); + } + } - // if multiple lane change is necessary, does the remaining distance is sufficient - const auto remaining_dist_in_target = std::invoke([&]() { - const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; - const auto num_to_preferred_lane_from_target_lane = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const auto backward_len_buffer = - lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto backward_buffer_to_target_lane = - num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer; - return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane + - next_lane_change_buffer; - }); - - if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } + RCLCPP_DEBUG(logger_, "No safety path found."); + return false; +} - const auto terminal_lane_changing_velocity = std::min( - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, - getCommonParam().max_vel); - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); +LaneChangePath NormalLaneChange::get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double shift_length) const +{ + const auto & route_handler = *getRouteHandler(); + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, - initial_lane_changing_velocity, next_lane_change_buffer); + const auto resample_interval = + utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); - if (target_segment.points.empty()) { - debug_print_lat("Reject: target segment is empty!! something wrong..."); - continue; - } + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("target_lane_reference_path is empty!"); + } - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, initial_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose, target_lane_length, - lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); - - if (target_lane_reference_path.points.empty()) { - debug_print_lat("Reject: target_lane_reference_path is empty!!"); - continue; - } + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); + const auto shift_line = utils::lane_change::get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - const auto candidate_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, prepare_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - if (!candidate_path) { - debug_print_lat("Reject: failed to generate candidate path!!"); - continue; - } + const auto candidate_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print_lat("Reject: invalid candidate path!!"); - continue; - } + if (!candidate_path) { + throw std::logic_error("failed to generate candidate path!"); + } - candidate_paths->push_back(*candidate_path); + if ( + candidate_path.value().info.length.sum() + + common_data_ptr_->transient_data.next_dist_buffer.min > + common_data_ptr_->transient_data.dist_to_terminal_end) { + throw std::logic_error("invalid candidate path length!"); + } - if ( - !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { - debug_print_lat( - "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " - "lane change."); - return false; - } + return *candidate_path; +} - const auto is_safe = std::invoke([&]() { - constexpr size_t decel_sampling_num = 1; - const auto safety_check_with_normal_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); +bool NormalLaneChange::check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const +{ + const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; + if (utils::lane_change::has_overtaking_turn_lane_object( + common_data_ptr_, filtered_objects_.target_lane_trailing)) { + throw std::logic_error("Ego is nearby intersection, and there might be overtaking vehicle."); + } - if (!safety_check_with_normal_rss.is_safe && is_stuck) { - const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); - return safety_check_with_stuck_rss.is_safe; - } + if ( + !is_stuck && utils::lane_change::is_delay_lane_change( + common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, + lane_change_debug_.collision_check_objects)) { + throw std::logic_error( + "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); + } - return safety_check_with_normal_rss.is_safe; - }); + const auto lc_start_velocity = candidate_path.info.velocity.prepare; + const auto min_lc_velocity = lane_change_parameters_->trajectory.min_lane_changing_velocity; + constexpr double margin = 0.1; + // path is unsafe if it exceeds target lane boundary with a high velocity + if ( + lane_change_parameters_->safety.enable_target_lane_bound_check && + lc_start_velocity > min_lc_velocity + margin && + utils::lane_change::path_footprint_exceeds_target_lane_bound( + common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) { + throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); + } - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } + constexpr size_t decel_sampling_num = 1; + const auto safety_check_with_normal_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params, + decel_sampling_num, lane_change_debug_.collision_check_objects); - debug_print_lat("Reject: sampled path is not safe."); - } - } + if (!safety_check_with_normal_rss.is_safe && is_stuck) { + const auto safety_check_with_stuck_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, + decel_sampling_num, lane_change_debug_.collision_check_objects); + return safety_check_with_stuck_rss.is_safe; } - RCLCPP_DEBUG(logger_, "No safety path found."); - return false; + return safety_check_with_normal_rss.is_safe; } std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + const auto is_empty = [&](const auto & data, const auto & s) { + if (!data.empty()) return false; + RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); + return true; + }; + + const auto target_lane_neighbors_polygon_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; + if ( + is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || + is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { return {}; } + const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto forward_path_length = common_parameters.forward_path_length; const auto minimum_lane_changing_velocity = - lane_change_parameters_->minimum_lane_changing_velocity; + lane_change_parameters_->trajectory.min_lane_changing_velocity; const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); - - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - - const auto target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return {}; - } + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = @@ -1752,7 +1430,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( prev_module_output_.path.points, current_lane_terminal_point, - -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); @@ -1772,41 +1450,33 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( target_lanes, lane_changing_start_pose.value()); const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + lane_change_parameters_->trajectory.lat_acc_map.find(minimum_lane_changing_velocity); const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + shift_length, lane_change_parameters_->trajectory.lateral_jerk, max_lateral_acc); + const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, - minimum_lane_changing_velocity, next_lane_change_buffer); + target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, + minimum_lane_changing_velocity, next_min_dist_buffer); if (target_segment.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); return {}; } - const lanelet::BasicPoint2d lc_start_point( - lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); - - const auto & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); - - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - LaneChangeInfo lane_change_info; lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; lane_change_info.velocity = LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.length = LaneChangePhaseInfo{0.0, current_min_dist_buffer}; lane_change_info.lane_changing_start = lane_changing_start_pose.value(); lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = max_lateral_acc; lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; - if (!is_valid_start_point) { + if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { RCLCPP_DEBUG( logger_, "Reject: lane changing points are not inside of the target preferred lanes or its " @@ -1815,18 +1485,16 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_change_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); + current_min_dist_buffer, minimum_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); return {}; } - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( lane_changing_start_pose.value(), target_segment.points.front().point.pose, target_lane_reference_path, shift_length); @@ -1837,11 +1505,12 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); // remove terminal points because utils::clipPathLength() calculates extra long path reference_segment.points.pop_back(); - reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + reference_segment.points.back().point.longitudinal_velocity_mps = + static_cast(minimum_lane_changing_velocity); - const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, reference_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, + sorted_lane_ids); return terminal_lane_change_path; } @@ -1856,24 +1525,25 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {true, true}; } - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); + const auto target_objects = get_target_objects(filtered_objects_, current_lanes); CollisionCheckDebugMap debug_data; - const auto min_lc_length = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); + const auto has_overtaking_object = utils::lane_change::has_overtaking_turn_lane_object( + common_data_ptr_, filtered_objects_.target_lane_trailing); - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data); + if (has_overtaking_object) { + return {false, true}; + } - if (!has_passed_parked_objects) { + if (utils::lane_change::is_delay_lane_change( + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data)) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); return {false, false}; } const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, + path, target_objects, lane_change_parameters_->safety.rss_params_for_abort, static_cast(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data); { // only for debug purpose @@ -1907,7 +1577,7 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( } unsafe_hysteresis_count_ = 0; } - if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) { + if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.th_unsafe_hysteresis) { RCLCPP_DEBUG( logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__, (approved_path_safety_status.is_safe ? "safe" : "UNSAFE")); @@ -1947,20 +1617,15 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const } // check relative angle - if (!utils::checkPathRelativeAngle(path, M_PI)) { - return false; - } - - return true; + return utils::checkPathRelativeAngle(path, M_PI); } bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_trailing_object) { + common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && + is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -1974,23 +1639,21 @@ bool NormalLaneChange::calcAbortPath() const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = - std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); + std::max(lane_change_parameters_->trajectory.min_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; - const auto direction = getDirection(); - const auto minimum_lane_change_length = calculation::calc_minimum_lane_change_length( - route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto & lane_changing_path = selected_path.path; const auto & reference_lanelets = get_current_lanes(); const auto lane_changing_end_pose_idx = std::invoke([&]() { constexpr double s_start = 0.0; const double s_end = std::max( - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, 0.0); + lanelet::utils::getLaneletLength2d(reference_lanelets) - current_min_dist_buffer, 0.0); const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( @@ -2030,19 +1693,10 @@ bool NormalLaneChange::calcAbortPath() return false; } - const auto dist_to_terminal_start = std::invoke([&]() { - const auto dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, get_current_lanes(), common_data_ptr_->get_ego_pose()); - const auto min_lc_length = calculation::calc_minimum_lane_change_length( - route_handler, get_current_lanes().back(), *common_data_ptr_->lc_param_ptr, - common_data_ptr_->direction); - return dist_to_terminal_end - min_lc_length; - }); - const auto enough_abort_dist = abort_start_dist + abort_return_dist + calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <= - dist_to_terminal_start; + common_data_ptr_->transient_data.dist_to_terminal_start; if (!enough_abort_dist) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); @@ -2069,7 +1723,7 @@ bool NormalLaneChange::calcAbortPath() shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); const auto lateral_acc_range = - lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); + lane_change_parameters_->trajectory.lat_acc_map.find(current_velocity); const double & max_lateral_acc = lateral_acc_range.second; path_shifter.setLateralAccelerationLimit(max_lateral_acc); @@ -2108,7 +1762,7 @@ bool NormalLaneChange::calcAbortPath() PathWithLaneId aborting_path; aborting_path.points.insert( aborting_path.points.begin(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_return_idx); + shifted_path.path.points.begin() + static_cast(abort_return_idx)); if (!reference_lane_segment.points.empty()) { abort_path.path = utils::combinePath(aborting_path, reference_lane_segment); @@ -2187,16 +1841,16 @@ bool NormalLaneChange::has_collision_with_decel_patterns( acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), [&](double n) { return lane_changing_acc + n * acc_resolution; }); + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; const auto all_collided = std::all_of( acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( common_data_ptr_, lane_change_path, acceleration); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = - (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_param; + const auto selected_rss_param = (obj.initial_twist.linear.x <= stopped_obj_vel_th) + ? lane_change_parameters_->safety.rss_params_for_parked + : rss_param; return is_collided( lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, debug_data); @@ -2226,7 +1880,7 @@ bool NormalLaneChange::is_collided( const auto & current_polygon = lanes_polygon_ptr->current; const auto & expanded_target_polygon = lanes_polygon_ptr->target; - if (!current_polygon.has_value() || !expanded_target_polygon.has_value()) { + if (current_polygon.empty() || expanded_target_polygon.empty()) { return !is_collided; } @@ -2235,11 +1889,11 @@ bool NormalLaneChange::is_collided( constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - obj, lane_change_parameters_->use_all_predicted_path); + obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); const auto safety_check_max_vel = get_max_velocity_for_safety_check(); const auto & bpp_param = *common_data_ptr_->bpp_param_ptr; - const double velocity_threshold = lane_change_parameters_->stopped_object_velocity_threshold; + const double velocity_threshold = lane_change_parameters_->safety.th_stopped_object_velocity; const double prepare_duration = lane_change_path.info.duration.prepare; const double start_time = check_prepare_phase ? 0.0 : prepare_duration; @@ -2266,9 +1920,9 @@ bool NormalLaneChange::is_collided( } const auto collision_in_current_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_polygon); - const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); + utils::lane_change::is_collided_polygons_in_lanelet(collided_polygons, current_polygon); + const auto collision_in_target_lanes = utils::lane_change::is_collided_polygons_in_lanelet( + collided_polygons, expanded_target_polygon); if (!collision_in_current_lanes && !collision_in_target_lanes) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -2284,64 +1938,6 @@ bool NormalLaneChange::is_collided( return !is_collided; } -// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes -bool NormalLaneChange::isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // 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; - } - - // 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 : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point - - // Note: it needs chattering prevention. - if (std::abs(object.initial_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) { - RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); - return true; // Stationary object is in front of ego. - } - } - - // Check if Ego is in terminal of current lanes - const auto & route_handler = getRouteHandler(); - const double distance_to_terminal = - route_handler->isInGoalRouteSection(current_lanes.back()) - ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) - : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; - if (distance_to_terminal < terminal_judge_buffer) { - return true; - } - - // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current - RCLCPP_DEBUG( - logger_, - "No stationary objects found in obstacle_check_distance and Ego is not in " - "terminal of current lanes"); - return false; -} - double NormalLaneChange::get_max_velocity_for_safety_check() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2354,35 +1950,56 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const return getCommonParam().max_vel; } -bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::is_ego_stuck() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (current_lanes.empty()) { - lane_change_debug_.is_stuck = false; - return false; // can not check + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->th_stop_velocity) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; } - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto max_lane_change_length = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lc_param_ptr->th_stop_time) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + const auto & current_lanes_path = common_data_ptr_->current_lanes_path; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); + 0.0, lc_param_ptr->trajectory.min_lane_changing_velocity, lc_param_ptr->safety.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_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + constexpr auto detection_distance_margin = 10.0; + const auto obstacle_check_distance = common_data_ptr_->transient_data.lane_changing_length.max + + rss_dist + common_data_ptr_->bpp_param_ptr->base_link2front + + detection_distance_margin; + const auto has_object_blocking = std::any_of( + filtered_objects_.current_lane.begin(), filtered_objects_.current_lane.end(), + [&](const auto & object) { + // Note: it needs chattering prevention. + if ( + std::abs(object.initial_twist.linear.x) > + lc_param_ptr->safety.th_stopped_object_velocity) { // check if stationary + return false; + } - auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + const auto ego_to_obj_dist = + calcSignedArcLength( + current_lanes_path.points, ego_pose.position, object.initial_pose.position) - + obstacle_check_distance; + return ego_to_obj_dist < 0.0; + }); - lane_change_debug_.is_stuck = is_vehicle_stuck; - return is_vehicle_stuck; + lane_change_debug_.is_stuck = has_object_blocking; + return has_object_blocking; } bool NormalLaneChange::is_valid_start_point( @@ -2391,15 +2008,16 @@ bool NormalLaneChange::is_valid_start_point( const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; - const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target.value(); + const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target; return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || boost::geometry::covered_by(lc_start_point, target_lane_poly); } -void NormalLaneChange::setStopPose(const Pose & stop_pose) +void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) { - lane_change_stop_pose_ = stop_pose; + const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); + lane_change_stop_pose_ = stop_point.point.pose; } void NormalLaneChange::updateStopTime() @@ -2407,14 +2025,14 @@ void NormalLaneChange::updateStopTime() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_vel = getEgoVelocity(); - if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { + if (std::abs(current_vel) > lane_change_parameters_->th_stop_velocity) { stop_time_ = 0.0; } else { const double duration = stop_watch_.toc("stop_time"); // clip stop time - if (stop_time_ + duration * 0.001 > lane_change_parameters_->stop_time_threshold) { + if (stop_time_ + duration * 0.001 > lane_change_parameters_->th_stop_time) { constexpr double eps = 0.1; - stop_time_ = lane_change_parameters_->stop_time_threshold + eps; + stop_time_ = lane_change_parameters_->th_stop_time + eps; } else { stop_time_ += duration * 0.001; } @@ -2426,10 +2044,9 @@ void NormalLaneChange::updateStopTime() bool NormalLaneChange::check_prepare_phase() const { const auto & route_handler = getRouteHandler(); - const auto & vehicle_info = getCommonParam().vehicle_info; const auto check_in_general_lanes = - lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_general_lanes; lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { @@ -2439,24 +2056,52 @@ bool NormalLaneChange::check_prepare_phase() const return check_in_general_lanes; } - const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + const auto check_in_intersection = + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_intersection && + common_data_ptr_->transient_data.in_intersection; - const auto check_in_intersection = std::invoke([&]() { - if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { - return false; - } + const auto check_in_turns = + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_turns && + common_data_ptr_->transient_data.in_turn_direction_lane; - return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); - }); + return check_in_intersection || check_in_turns || check_in_general_lanes; +} - const auto check_in_turns = std::invoke([&]() { - if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) { - return false; - } +void NormalLaneChange::update_dist_from_intersection() +{ + auto & transient_data = common_data_ptr_->transient_data; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); - }); + if ( + transient_data.in_intersection && transient_data.in_turn_direction_lane && + path_after_intersection_.empty()) { + auto path_after_intersection = route_handler_ptr->getCenterLinePath( + common_data_ptr_->lanes_ptr->target_neighbor, 0.0, std::numeric_limits::max()); + path_after_intersection_ = std::move(path_after_intersection.points); + transient_data.dist_from_prev_intersection = 0.0; + return; + } - return check_in_intersection || check_in_turns || check_in_general_lanes; + if ( + transient_data.in_intersection || transient_data.in_turn_direction_lane || + path_after_intersection_.empty()) { + return; + } + + const auto & path_points = path_after_intersection_; + const auto & front_point = path_points.front().point.pose.position; + const auto & ego_position = common_data_ptr_->get_ego_pose().position; + transient_data.dist_from_prev_intersection = + calcSignedArcLength(path_points, front_point, ego_position); + + if ( + transient_data.dist_from_prev_intersection >= 0.0 && + transient_data.dist_from_prev_intersection <= + common_data_ptr_->lc_param_ptr->backward_length_from_intersection) { + return; + } + + path_after_intersection_.clear(); + transient_data.dist_from_prev_intersection = std::numeric_limits::max(); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 1d656a5ee698f..ad8279cde78ae 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -16,15 +16,16 @@ #include #include +#include + namespace autoware::behavior_path_planner::utils::lane_change::calculation { -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) -{ - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & current_lanes = lanes_ptr->current; - const auto & current_pose = common_data_ptr->get_ego_pose(); - return calc_dist_from_pose_to_terminal_end(common_data_ptr, current_lanes, current_pose); +rclcpp::Logger get_logger() +{ + constexpr const char * name{"lane_change.utils"}; + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; } double calc_dist_from_pose_to_terminal_end( @@ -35,10 +36,10 @@ double calc_dist_from_pose_to_terminal_end( return 0.0; } - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - - if (lanes_ptr->current_lane_in_goal_section) { + const auto in_goal_route_section = + common_data_ptr->route_handler_ptr->isInGoalRouteSection(lanes.back()); + if (in_goal_route_section) { + const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); return utils::getSignedDistance(src_pose, goal_pose, lanes); } return utils::getDistanceToEndOfLane(src_pose, lanes); @@ -47,17 +48,62 @@ double calc_dist_from_pose_to_terminal_end( double calc_stopping_distance(const LCParamPtr & lc_param_ptr) { // v^2 = u^2 + 2ad - const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity; - const auto min_lon_acc = lc_param_ptr->min_longitudinal_acc; + const auto min_lc_vel = lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto min_lon_acc = lc_param_ptr->trajectory.min_longitudinal_acc; const auto min_back_dist = std::abs((min_lc_vel * min_lc_vel) / (2 * min_lon_acc)); const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; return std::max(min_back_dist, param_back_dist); } +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; + universe_utils::MultiPolygon2d center_line_polygon; + namespace strategy = boost::geometry::strategy::buffer; + boost::geometry::buffer( + line_string, center_line_polygon, strategy::distance_symmetric(buffer_distance), + strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), + strategy::point_square()); + + if (center_line_polygon.empty()) return 0.0; + + std::vector intersection_points; + boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + + if (intersection_points.empty()) { + return utils::getDistanceToEndOfLane(src_pose, lanelets); + } + + Pose pose; + double distance = std::numeric_limits::max(); + for (const auto & point : intersection_points) { + pose.position.x = boost::geometry::get<0>(point); + pose.position.y = boost::geometry::get<1>(point); + distance = std::min(distance, utils::getSignedDistance(src_pose, pose, lanelets)); + } + + return std::max(distance - (bpp_param.base_link2front + margin), 0.0); +} + double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { - const auto max_prepare_duration = common_data_ptr->lc_param_ptr->lane_change_prepare_duration; + const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel; return max_prepare_duration * ego_max_speed; @@ -82,8 +128,7 @@ double calc_ego_dist_to_lanes_start( return std::numeric_limits::max(); } - const auto path = - route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr->current_lanes_path; if (path.points.empty()) { return std::numeric_limits::max(); @@ -95,93 +140,405 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) +double calc_minimum_acceleration( + const LaneChangeParameters & lane_change_parameters, const double current_velocity, + const double min_acc_threshold, const double prepare_duration) +{ + if (prepare_duration < eps) return -std::abs(min_acc_threshold); + const double min_lc_velocity = lane_change_parameters.trajectory.min_lane_changing_velocity; + const double acc = (min_lc_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, -std::abs(min_acc_threshold), -eps); +} + +double calc_maximum_acceleration( + const double prepare_duration, const double current_velocity, const double current_max_velocity, + const double max_acc_threshold) +{ + if (prepare_duration < eps) return max_acc_threshold; + const double acc = (current_max_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, 0.0, max_acc_threshold); +} + +std::vector calc_min_lane_change_lengths( + const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) { if (shift_intervals.empty()) { - return 0.0; + return {}; } - const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto min_max_lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(min_vel); - // const auto min_lat_acc = std::get<0>(min_max_lat_acc); + const auto min_vel = lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto min_max_lat_acc = lc_param_ptr->trajectory.lat_acc_map.find(min_vel); const auto max_lat_acc = std::get<1>(min_max_lat_acc); - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const auto lat_jerk = lc_param_ptr->trajectory.lateral_jerk; + + std::vector min_lc_lengths{}; + min_lc_lengths.reserve(shift_intervals.size()); - const auto calc_sum = [&](double sum, double shift_interval) { + const auto min_lc_length = [&](const auto shift_interval) { const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); - return sum + (min_vel * t + finish_judge_buffer); + return min_vel * t; }; - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(min_lc_lengths), + min_lc_length); - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); + return min_lc_lengths; } -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction) +std::vector calc_max_lane_change_lengths( + const CommonDataPtr & common_data_ptr, const std::vector & shift_intervals) { - if (!route_handler) { - return std::numeric_limits::max(); + if (shift_intervals.empty()) { + return {}; } - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction); - return calc_minimum_lane_change_length(lane_change_parameters, shift_intervals); -} + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto lat_jerk = params.lateral_jerk; + const auto t_prepare = params.max_prepare_duration; + const auto current_velocity = common_data_ptr->get_ego_speed(); + const auto path_velocity = common_data_ptr->transient_data.current_path_velocity; -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc) -{ - if (shift_intervals.empty()) { - return 0.0; - } + const auto max_acc = calc_maximum_acceleration( + t_prepare, current_velocity, path_velocity, params.max_longitudinal_acc); - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto t_prepare = lane_change_parameters.lane_change_prepare_duration; + // TODO(Quda, Azu): should probably limit upper bound of velocity as well, but + // disabled due failing evaluation tests. + // const auto limit_vel = [&](const auto vel) { + // const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; + // return std::clamp(vel, params.min_lane_changing_velocity, max_global_vel); + // }; - auto vel = current_velocity; + auto vel = std::max(common_data_ptr->get_ego_speed(), params.min_lane_changing_velocity); - const auto calc_sum = [&](double sum, double shift_interval) { + std::vector max_lc_lengths{}; + + const auto max_lc_length = [&](const auto shift_interval) { // prepare section const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; vel = vel + max_acc * t_prepare; // lane changing section - const auto [min_lat_acc, max_lat_acc] = - lane_change_parameters.lane_change_lat_acc_map.find(vel); + const auto [min_lat_acc, max_lat_acc] = params.lat_acc_map.find(vel); const auto t_lane_changing = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; vel = vel + max_acc * t_lane_changing; - return sum + (prepare_length + lane_changing_length + finish_judge_buffer); + return prepare_length + lane_changing_length; }; - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(max_lc_lengths), + max_lc_length); + return max_lc_lengths; +} - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); +double calc_distance_buffer(const LCParamPtr & lc_param_ptr, const std::vector & lc_lengths) +{ + if (lc_lengths.empty()) { + return std::numeric_limits::max(); + } + + const auto finish_judge_buffer = lc_param_ptr->lane_change_finish_judge_buffer; + const auto backward_buffer = calc_stopping_distance(lc_param_ptr); + const auto lengths_sum = std::accumulate(lc_lengths.begin(), lc_lengths.end(), 0.0); + const auto num_of_lane_changes = static_cast(lc_lengths.size()); + return lengths_sum + (num_of_lane_changes * finish_judge_buffer) + + ((num_of_lane_changes - 1.0) * backward_buffer); } -double calc_maximum_lane_change_length( - const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, - const double max_acc) +std::vector calc_shift_intervals( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) { - const auto shift_intervals = - common_data_ptr->route_handler_ptr->getLateralIntervalsToPreferredLane( - current_terminal_lanelet); - const auto vel = std::max( - common_data_ptr->get_ego_speed(), - common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity); - return calc_maximum_lane_change_length( - vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto direction = common_data_ptr->direction; + + return route_handler_ptr->getLateralIntervalsToPreferredLane(lanes.back(), direction); +} + +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) +{ + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + const auto shift_intervals = calculation::calc_shift_intervals(common_data_ptr, lanes); + const auto min_lc_lengths = + calculation::calc_min_lane_change_lengths(common_data_ptr->lc_param_ptr, shift_intervals); + const auto min_lc_length = + !min_lc_lengths.empty() ? min_lc_lengths.front() : std::numeric_limits::max(); + const auto min_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, min_lc_lengths); + + const auto max_lc_lengths = + calculation::calc_max_lane_change_lengths(common_data_ptr, shift_intervals); + const auto max_lc_length = + !max_lc_lengths.empty() ? max_lc_lengths.front() : std::numeric_limits::max(); + const auto max_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, max_lc_lengths); + + return {{min_lc_length, max_lc_length}, {min_dist_buffer, max_dist_buffer}}; +} + +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} + +std::pair calc_min_max_acceleration( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration) +{ + const auto & lc_params = *common_data_ptr->lc_param_ptr; + const auto & bpp_params = *common_data_ptr->bpp_param_ptr; + const auto current_ego_velocity = common_data_ptr->get_ego_speed(); + + const auto min_accel_threshold = + std::max(bpp_params.min_acc, lc_params.trajectory.min_longitudinal_acc); + const auto max_accel_threshold = + std::min(bpp_params.max_acc, lc_params.trajectory.max_longitudinal_acc); + + // calculate minimum and maximum acceleration + const auto min_acc = calc_minimum_acceleration( + lc_params, current_ego_velocity, min_accel_threshold, prepare_duration); + const auto max_acc = calc_maximum_acceleration( + prepare_duration, current_ego_velocity, max_path_velocity, max_accel_threshold); + + return {min_acc, max_acc}; +} + +std::vector calc_acceleration_values( + const double min_accel, const double max_accel, const double sampling_num) +{ + if (min_accel > max_accel) return {}; + + if (max_accel - min_accel < eps) { + return {min_accel}; + } + + const auto resolution = std::abs(max_accel - min_accel) / sampling_num; + + std::vector sampled_values{min_accel}; + for (double accel = min_accel + resolution; accel < max_accel + eps; accel += resolution) { + // check whether if we need to add 0.0 + if (sampled_values.back() < -eps && accel > eps) { + sampled_values.push_back(0.0); + } + + sampled_values.push_back(accel); + } + std::reverse(sampled_values.begin(), sampled_values.end()); + + return sampled_values; +} + +std::vector calc_lon_acceleration_samples( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration) +{ + const auto & transient_data = common_data_ptr->transient_data; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); + const auto sampling_num = common_data_ptr->lc_param_ptr->trajectory.lon_acc_sampling_num; + + const auto min_max_accel = + calc_min_max_acceleration(common_data_ptr, max_path_velocity, prepare_duration); + const auto & min_accel = min_max_accel.first; + const auto & max_accel = min_max_accel.second; + + const auto is_sampling_required = std::invoke([&]() -> bool { + if (max_accel < 0.0 || transient_data.is_ego_stuck) return true; + + const auto max_dist_buffer = transient_data.current_dist_buffer.max; + if (max_dist_buffer > transient_data.dist_to_terminal_end) return true; + + const auto dist_to_target_lane_end = + common_data_ptr->lanes_ptr->target_lane_in_goal_section + ? utils::getSignedDistance(current_pose, goal_pose, target_lanes) + : utils::getDistanceToEndOfLane(current_pose, target_lanes); + + return max_dist_buffer >= dist_to_target_lane_end; + }); + + if (is_sampling_required) { + return calc_acceleration_values(min_accel, max_accel, sampling_num); + } + + return {max_accel}; +} + +double calc_lane_changing_acceleration( + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc) +{ + if (prepare_longitudinal_acc <= 0.0) { + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto lane_changing_acc = + common_data_ptr->transient_data.is_ego_near_current_terminal_start + ? prepare_longitudinal_acc * params.lane_changing_decel_factor + : 0.0; + return lane_changing_acc; + } + + return std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + prepare_longitudinal_acc); +} + +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration) +{ + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_velocity = params.min_lane_changing_velocity; + + // need to ensure min prep duration is sufficient to reach minimum lane changing velocity + const auto min_prepare_duration = std::invoke([&]() -> double { + if (current_velocity >= min_lc_velocity) { + return params.min_prepare_duration; + } + const auto max_acc = + std::min(common_data_ptr->bpp_param_ptr->max_acc, params.max_longitudinal_acc); + if (max_acc < eps) { + return params.max_prepare_duration; + } + return (min_lc_velocity - current_velocity) / max_acc; + }); + + return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration); +} + +std::vector calc_prepare_durations(const CommonDataPtr & common_data_ptr) +{ + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) { + return {common_data_ptr->transient_data.lane_change_prepare_duration}; + } + + const auto max_prepare_duration = lc_param_ptr->trajectory.max_prepare_duration; + std::vector prepare_durations; + constexpr double step = 0.5; + + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { + prepare_durations.push_back(duration); + } + + return prepare_durations; +} + +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double max_path_velocity, const double min_length_threshold, + const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + std::vector metrics; + + auto is_skip = [&](const double prepare_length) { + if (prepare_length > max_length_threshold || prepare_length < min_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: prepare length out of expected range. length: %.5f, threshold min: %.5f, max: %.5f", + prepare_length, min_length_threshold, max_length_threshold); + return true; + } + return false; + }; + + const auto prepare_durations = calc_prepare_durations(common_data_ptr); + + for (const auto & prepare_duration : prepare_durations) { + const auto lon_accel_samples = + calc_lon_acceleration_samples(common_data_ptr, max_path_velocity, prepare_duration); + for (const auto & lon_accel : lon_accel_samples) { + const auto prepare_velocity = + std::clamp(current_velocity + lon_accel * prepare_duration, min_lc_vel, max_vel); + + // compute actual longitudinal acceleration + const double prepare_accel = (prepare_duration < 1e-3) + ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); + + const auto prepare_length = + calc_phase_length(current_velocity, max_vel, prepare_accel, prepare_duration); + + if (is_skip(prepare_length)) continue; + + metrics.emplace_back( + prepare_duration, prepare_length, prepare_velocity, lon_accel, prepare_accel, 0.0); + } + } + + return metrics; +} + +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_path_velocity, const double lon_accel, const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_data_ptr->lc_param_ptr->trajectory.lat_acc_map.find(initial_velocity); + const auto lateral_acc_resolution = + std::abs(max_lateral_acc - min_lateral_acc) / + common_data_ptr->lc_param_ptr->trajectory.lat_acc_sampling_num; + + std::vector metrics; + + auto is_skip = [&](const double lane_changing_length) { + if (lane_changing_length > max_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: lane changing length exceeds maximum threshold. length: %.5f, threshold: %.5f", + lane_changing_length, max_length_threshold); + return true; + } + return false; + }; + + for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; + lat_acc += lateral_acc_resolution) { + const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( + shift_length, common_data_ptr->lc_param_ptr->trajectory.lateral_jerk, lat_acc); + + const double lane_changing_accel = calc_lane_changing_acceleration( + common_data_ptr, initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); + + const auto lane_changing_length = calculation::calc_phase_length( + initial_velocity, max_vel, lane_changing_accel, lane_changing_duration); + + if (is_skip(lane_changing_length)) continue; + + const auto lane_changing_velocity = std::clamp( + initial_velocity + lane_changing_accel * lane_changing_duration, min_lc_vel, max_vel); + + metrics.emplace_back( + lane_changing_duration, lane_changing_length, lane_changing_velocity, lon_accel, + lane_changing_accel, lat_acc); + } + + return metrics; } } // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index eb64493b0c728..5195d4ba267fa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -14,7 +14,6 @@ #include "autoware/behavior_path_planner_common/marker_utils/colors.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include #include @@ -101,36 +100,34 @@ MarkerArray createLaneChangingVirtualWallMarker( } MarkerArray showFilteredObjects( - const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns) + const FilteredLanesObjects & filtered_objects, const std::string & ns) { int32_t update_id = 0; - auto current_marker = marker_utils::showFilteredObjects( - filtered_objects.current_lane, ns, colors::yellow(), update_id); - update_id += static_cast(current_marker.markers.size()); - auto target_leading_marker = marker_utils::showFilteredObjects( - filtered_objects.target_lane_leading, ns, colors::aqua(), update_id); - update_id += static_cast(target_leading_marker.markers.size()); - auto target_trailing_marker = marker_utils::showFilteredObjects( - filtered_objects.target_lane_trailing, ns, colors::blue(), update_id); - update_id += static_cast(target_trailing_marker.markers.size()); - auto other_marker = marker_utils::showFilteredObjects( - filtered_objects.other_lane, ns, colors::medium_orchid(), update_id); - MarkerArray marker_array; - std::move( - current_marker.markers.begin(), current_marker.markers.end(), - std::back_inserter(marker_array.markers)); - std::move( - target_leading_marker.markers.begin(), target_leading_marker.markers.end(), - std::back_inserter(marker_array.markers)); - - std::move( - target_trailing_marker.markers.begin(), target_trailing_marker.markers.end(), - std::back_inserter(marker_array.markers)); - - std::move( - other_marker.markers.begin(), other_marker.markers.end(), - std::back_inserter(marker_array.markers)); + auto reserve_size = filtered_objects.current_lane.size() + filtered_objects.others.size() + + filtered_objects.target_lane_leading.size() + + filtered_objects.target_lane_trailing.size(); + marker_array.markers.reserve(2 * reserve_size); + auto add_objects_to_marker = + [&](const ExtendedPredictedObjects & objects, const ColorRGBA & color) { + if (objects.empty()) { + return; + } + + auto marker = marker_utils::showFilteredObjects(objects, ns, color, update_id); + update_id += static_cast(marker.markers.size()); + std::move( + marker.markers.begin(), marker.markers.end(), std::back_inserter(marker_array.markers)); + }; + + add_objects_to_marker(filtered_objects.current_lane, colors::yellow()); + add_objects_to_marker(filtered_objects.target_lane_leading.moving, colors::aqua()); + add_objects_to_marker(filtered_objects.target_lane_leading.stopped, colors::light_steel_blue()); + add_objects_to_marker(filtered_objects.target_lane_trailing, colors::blue()); + add_objects_to_marker( + filtered_objects.target_lane_leading.stopped_at_bound, colors::light_pink()); + add_objects_to_marker(filtered_objects.others, colors::medium_orchid()); + return marker_array; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index a063919a1db5a..e30e5c4ff2f83 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -14,10 +14,8 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" -#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" -#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -27,6 +25,7 @@ #include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include #include #include #include @@ -35,6 +34,12 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include #include @@ -52,16 +57,15 @@ #include #include #include -#include #include #include +#include #include namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; @@ -74,7 +78,14 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { constexpr const char * name{"lane_change.utils"}; - return rclcpp::get_logger(name); + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; +} + +bool is_mandatory_lane_change(const ModuleType lc_type) +{ + return lc_type == LaneChangeModuleType::NORMAL || + lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; } double calcLaneChangeResampleInterval( @@ -86,47 +97,14 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMinimumAcceleration( - const double current_velocity, const double min_longitudinal_acc, - const LaneChangeParameters & lane_change_parameters) -{ - const double min_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; - const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); -} - -double calcMaximumAcceleration( - const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) -{ - const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const double acc = (current_max_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, 0.0, max_longitudinal_acc); -} - -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc) -{ - if (prepare_longitudinal_acc <= 0.0) { - return 0.0; - } - - return std::clamp( - (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, - prepare_longitudinal_acc); -} - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { if (current_velocity < prepare_velocity) { // acceleration - for (size_t i = 0; i < prepare_segment.points.size(); ++i) { - prepare_segment.points.at(i).point.longitudinal_velocity_mps = std::min( - prepare_segment.points.at(i).point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); + for (auto & point : prepare_segment.points) { + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); } } else { // deceleration @@ -136,79 +114,20 @@ void setPrepareVelocity( } } -std::vector getAccelerationValues( - const double min_acc, const double max_acc, const size_t sampling_num) -{ - if (min_acc > max_acc) { - return {}; - } - - if (max_acc - min_acc < std::numeric_limits::epsilon()) { - return {0.0}; - } - - constexpr double epsilon = 0.001; - const auto resolution = std::abs(max_acc - min_acc) / sampling_num; - - std::vector sampled_values{min_acc}; - for (double sampled_acc = min_acc + resolution; - sampled_acc < max_acc + std::numeric_limits::epsilon(); sampled_acc += resolution) { - // check whether if we need to add 0.0 - if (sampled_values.back() < -epsilon && sampled_acc > epsilon) { - sampled_values.push_back(0.0); - } - - sampled_values.push_back(sampled_acc); - } - std::reverse(sampled_values.begin(), sampled_values.end()); - - return sampled_values; -} - -lanelet::ConstLanelets getTargetPreferredLanes( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction & direction, - const LaneChangeModuleType & type) -{ - if (type != LaneChangeModuleType::NORMAL) { - return target_lanes; - } - - const auto target_lane = - utils::lane_change::getLaneChangeTargetLane(route_handler, current_lanes, type, direction); - if (!target_lane) { - return target_lanes; - } - - const auto itr = std::find_if( - target_lanes.begin(), target_lanes.end(), - [&](const lanelet::ConstLanelet & lane) { return lane.id() == target_lane->id(); }); - - if (itr == target_lanes.end()) { - return target_lanes; - } - - const int target_id = std::distance(target_lanes.begin(), itr); - const lanelet::ConstLanelets target_preferred_lanes( - target_lanes.begin() + target_id, target_lanes.end()); - return target_preferred_lanes; -} - -lanelet::ConstLanelets getTargetNeighborLanes( +lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type) { lanelet::ConstLanelets neighbor_lanes; for (const auto & current_lane : current_lanes) { + const auto mandatory_lane_change = is_mandatory_lane_change(type); if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + if (mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } else { - if (type != LaneChangeModuleType::NORMAL) { + if (!mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } @@ -241,10 +160,39 @@ bool isPathInLanelets( return true; } -std::optional constructCandidatePath( +bool path_footprint_exceeds_target_lane_bound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin) +{ + if (common_data_ptr->direction == Direction::NONE || path.points.empty()) { + return false; + } + + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const bool is_left = common_data_ptr->direction == Direction::LEFT; + + const auto combined_target_lane = lanelet::utils::combineLaneletsShape(target_lanes); + + for (const auto & path_point : path.points) { + const auto & pose = path_point.point.pose; + const auto front_vertex = getEgoFrontVertex(pose, ego_info, is_left); + + const auto sign = is_left ? -1.0 : 1.0; + const auto dist_to_boundary = + sign * utils::getSignedDistanceFromLaneBoundary(combined_target_lane, front_vertex, is_left); + + if (dist_to_boundary < margin) { + RCLCPP_DEBUG(get_logger(), "Path footprint exceeds target lane boundary"); + return true; + } + } + + return false; +} + +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; @@ -296,8 +244,8 @@ std::optional constructCandidatePath( continue; } const auto nearest_idx = - autoware::motion_utils::findNearestIndex(target_segment.points, point.point.pose); - point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; } // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster @@ -334,12 +282,17 @@ std::optional constructCandidatePath( return std::optional{candidate_path}; } -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) { + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -349,10 +302,10 @@ PathWithLaneId getReferencePathFromTargetLane( if (is_goal_in_route) { const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lane_change_buffer; + next_lc_buffer; return std::min(dist_from_lc_start, s_goal); } - return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); }); constexpr double epsilon = 1e-4; @@ -367,18 +320,7 @@ PathWithLaneId getReferencePathFromTargetLane( lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); } -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length) -{ - const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; - - return getLaneChangingShiftLine( - lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); -} - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length) { @@ -443,7 +385,7 @@ std::vector generateDrivableLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) { const auto has_same_lane = - [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { + [](const lanelet::ConstLanelets & lanes, const lanelet::ConstLanelet & lane) { if (lanes.empty()) return false; const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); @@ -571,10 +513,13 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr) { + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto rough_shift_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; @@ -648,17 +593,16 @@ CandidateOutput assignToCandidate( return candidate_output; } -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction) +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { - return route_handler.getLaneChangeTarget(current_lanes, direction); + const auto direction = common_data_ptr->direction; + const auto route_handler_ptr = common_data_ptr->route_handler_ptr; + if (is_mandatory_lane_change(common_data_ptr->lc_type)) { + return route_handler_ptr->getLaneChangeTarget(current_lanes, direction); } - return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); + return route_handler_ptr->getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } std::vector convert_to_predicted_path( @@ -685,7 +629,7 @@ std::vector convert_to_predicted_path( const auto duration = lane_change_path.info.duration.sum(); const auto prepare_time = lane_change_path.info.duration.prepare; const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; - const auto resolution = lc_param_ptr->prediction_time_resolution; + const auto resolution = lc_param_ptr->safety.collision_check.prediction_time_resolution; std::vector predicted_path; // prepare segment @@ -733,9 +677,6 @@ bool isParkedObject( // +: object position // o: nearest point on centerline - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - const double object_vel_norm = std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { @@ -754,49 +695,15 @@ bool isParkedObject( const double lat_dist = autoware::motion_utils::calcLateralOffset(path.points, object_pose.position); - lanelet::BasicLineString2d bound; - double center_to_bound_buffer = 0.0; - if (lat_dist > 0.0) { - // left side vehicle - const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); - const auto most_left_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute most_left_sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - bound = most_left_lanelet.leftBound2d().basicLineString(); - if (most_left_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } else { - // right side vehicle - const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); - const auto most_right_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages( - most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute most_right_sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - bound = most_right_lanelet.rightBound2d().basicLineString(); - if (most_right_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } + const auto most_side_lanelet = + lat_dist > 0.0 ? route_handler.getMostLeftLanelet(closest_lanelet, false, true) + : route_handler.getMostRightLanelet(closest_lanelet, false, true); + const auto bound = lat_dist > 0.0 ? most_side_lanelet.leftBound2d().basicLineString() + : most_side_lanelet.rightBound2d().basicLineString(); + const lanelet::Attribute lanelet_sub_type = + most_side_lanelet.attribute(lanelet::AttributeName::Subtype); + const auto center_to_bound_buffer = + lanelet_sub_type.value() == "road_shoulder" ? 0.0 : object_check_min_road_shoulder_width; return isParkedObject( closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); @@ -841,153 +748,80 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passed_parked_objects( +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug) + const ExtendedPredictedObjects & target_objects, CollisionCheckDebugMap & object_debug) { - const auto route_handler = *common_data_ptr->route_handler_ptr; - const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters.object_check_min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = - lane_change_parameters.object_shiftable_ratio_threshold; - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - const auto current_lane_path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - - if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { - return true; - } - - const auto leading_obj_idx = getLeadingStaticObjectIdx( - route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold); - if (!leading_obj_idx) { - return true; - } + const auto & current_lane_path = common_data_ptr->current_lanes_path; + const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - const auto & leading_obj = objects.at(*leading_obj_idx); - auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); - const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); - if (leading_obj_poly.outer().empty()) { - return true; + if ( + !delay_lc_param.enable || target_objects.empty() || lane_change_path.path.points.empty() || + current_lane_path.points.empty()) { + return false; } - const auto & current_path_end = current_lane_path.points.back().point.pose.position; - const auto dist_to_path_end = [&](const auto & src_point) { - if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { - const auto goal_pose = route_handler.getGoalPose(); - return motion_utils::calcSignedArcLength( - current_lane_path.points, src_point, goal_pose.position); - } - return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end; + const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min; + auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) { + const auto dist_obj_to_end = dist_to_end - obj.dist_from_ego; + return dist_obj_to_end <= dist_buffer; }; - const auto min_dist_to_end_of_current_lane = std::invoke([&]() { - auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); - const auto dist = dist_to_path_end(obj_p); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - } - return min_dist_to_end_of_current_lane; - }); - - // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { - return true; - } - - const auto current_pose = common_data_ptr->get_ego_pose(); - const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); - - if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { - return true; - } - - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; -} - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) -{ - const auto & path = lane_change_path.path; - - if (path.points.empty() || objects.empty()) { - return {}; - } - - const auto & lane_change_start = lane_change_path.info.lane_changing_start; - const auto & path_end = path.points.back(); - - double dist_lc_start_to_leading_obj = 0.0; - std::optional leading_obj_idx = std::nullopt; - for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { - const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose; - - // ignore non-static object - // TODO(shimizu): parametrize threshold - const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - if (obj_vel_norm > 1.0) { - continue; - } + const auto ego_vel = common_data_ptr->get_ego_speed(); + const auto min_lon_acc = common_data_ptr->lc_param_ptr->trajectory.min_longitudinal_acc; + const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); + auto is_sufficient_gap = [&gap_threshold](const auto & current_obj, const auto & next_obj) { + const auto curr_obj_half_length = current_obj.shape.dimensions.x; + const auto next_obj_half_length = next_obj.shape.dimensions.x; + const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; + const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length; + return gap_length > gap_threshold; + }; - // ignore non-parked object - if (!isParkedObject( - path, route_handler, obj, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold)) { - continue; - } + for (auto it = target_objects.begin(); it < target_objects.end(); ++it) { + if (is_near_end(*it)) break; - const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, path_end.point.pose.position, obj_pose.position); - if (dist_back_to_obj > 0.0) { - // object is not on the lane change path - continue; - } + if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue; - const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, lane_change_start.position, obj_pose.position); - if (dist_lc_start_to_obj < 0.0) { - // object is on the lane changing path or behind it. It will be detected in safety check + if ( + delay_lc_param.check_only_parked_vehicle && + !isParkedObject( + lane_change_path.path, *common_data_ptr->route_handler_ptr, *it, + delay_lc_param.min_road_shoulder_width, delay_lc_param.th_parked_vehicle_shift_ratio)) { continue; } - if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { - dist_lc_start_to_leading_obj = dist_lc_start_to_obj; - leading_obj_idx = obj_idx; + auto next_it = std::next(it); + if (next_it == target_objects.end() || is_sufficient_gap(*it, *next_it)) { + auto debug = utils::path_safety_checker::createObjectDebug(*it); + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return true; } } - return leading_obj_idx; + return false; } -std::optional createPolygon( +lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) { if (lanes.empty()) { return {}; } + const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist); return lanelet::utils::to2D(polygon_3d).basicPolygon(); } ExtendedPredictedObject transform( - const PredictedObject & object, - [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object(object); - const auto & time_resolution = lane_change_parameters.prediction_time_resolution; + const auto & time_resolution = + lane_change_parameters.safety.collision_check.prediction_time_resolution; const double obj_vel_norm = std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); @@ -1013,12 +847,11 @@ ExtendedPredictedObject transform( return extended_object; } -bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, - const std::optional & lanes_polygon) +bool is_collided_polygons_in_lanelet( + const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon) { const auto is_in_lanes = [&](const auto & collided_polygon) { - return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + return !lanes_polygon.empty() && !boost::geometry::disjoint(collided_polygon, lanes_polygon); }; return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); @@ -1038,8 +871,7 @@ rclcpp::Logger getLogger(const std::string & type) return rclcpp::get_logger("lane_change").get_child(type); } -Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info) +Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info) { const auto base_to_front = ego_info.max_longitudinal_offset_m; const auto base_to_rear = ego_info.rear_overhang_m; @@ -1048,23 +880,40 @@ Polygon2d getEgoCurrentFootprint( return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); } -bool isWithinIntersection( +Point getEgoFrontVertex( + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info, bool left) +{ + const double lon_offset = ego_info.wheel_base_m + ego_info.front_overhang_m; + const double lat_offset = 0.5 * (left ? ego_info.vehicle_width_m : -ego_info.vehicle_width_m); + return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position; +} + +bool is_within_intersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) { const std::string id = lanelet.attributeOr("intersection_area", "else"); - if (id == "else") { + if (id == "else" || !std::atoi(id.c_str())) { return false; } - const auto lanelet_polygon = - route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + if (!route_handler || !route_handler->getLaneletMapPtr()) { + return false; + } + + const auto & polygon_layer = route_handler->getLaneletMapPtr()->polygonLayer; + const auto lanelet_polygon_opt = polygon_layer.find(std::atoi(id.c_str())); + if (lanelet_polygon_opt == polygon_layer.end()) { + return false; + } + const auto & lanelet_polygon = *lanelet_polygon_opt; return boost::geometry::within( polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); } -bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) +bool is_within_turn_direction_lanes( + const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) { const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); if (turn_direction == "else" || turn_direction == "straight") { @@ -1075,44 +924,34 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); } -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double duration) -{ - const auto length_with_acceleration = - velocity * duration + 0.5 * acceleration * std::pow(duration, 2); - const auto length_with_max_velocity = maximum_velocity * duration; - return std::min(length_with_acceleration, length_with_max_velocity); -} - LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { const auto & lanes = common_data_ptr->lanes_ptr; LanesPolygon lanes_polygon; lanes_polygon.current = - utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(lanes->current, 0.0, std::numeric_limits::max()); lanes_polygon.target = - utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(lanes->target, 0.0, std::numeric_limits::max()); - const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto & params = common_data_ptr->lc_param_ptr->safety; const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, - lc_param_ptr->lane_expansion_right_offset); - lanes_polygon.expanded_target = utils::lane_change::createPolygon( + lanes->target, common_data_ptr->direction, params.lane_expansion_left_offset, + params.lane_expansion_right_offset); + lanes_polygon.expanded_target = utils::lane_change::create_polygon( expanded_target_lanes, 0.0, std::numeric_limits::max()); - lanes_polygon.target_neighbor = *utils::lane_change::createPolygon( + lanes_polygon.target_neighbor = utils::lane_change::create_polygon( lanes->target_neighbor, 0.0, std::numeric_limits::max()); lanes_polygon.preceding_target.reserve(lanes->preceding_target.size()); for (const auto & preceding_lane : lanes->preceding_target) { auto lane_polygon = - utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(preceding_lane, 0.0, std::numeric_limits::max()); - if (lane_polygon) { - lanes_polygon.preceding_target.push_back(*lane_polygon); + if (!lane_polygon.empty()) { + lanes_polygon.preceding_target.push_back(lane_polygon); } } return lanes_polygon; @@ -1140,38 +979,22 @@ bool is_same_lane_with_prev_iteration( (prev_target_lanes.back().id() == prev_target_lanes.back().id()); } -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation) -{ - Pose pose; - pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - pose.orientation = orientation; - return pose; -} - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { - const auto & current_ego_pose = common_data_ptr->get_ego_pose(); - - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; - - const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength( - path.points, current_ego_pose.position, obj_position); const auto & ego_info = common_data_ptr->bpp_param_ptr->vehicle_info; const auto lon_dev = std::max( ego_info.max_longitudinal_offset_m + ego_info.rear_overhang_m, object.shape.dimensions.x); // we don't always have to check the distance accurately. - if (std::abs(dist_to_base_link) > lon_dev) { - return dist_to_base_link >= 0.0; + if (std::abs(object.dist_from_ego) > lon_dev) { + return object.dist_from_ego >= 0.0; } - const auto ego_polygon = getEgoCurrentFootprint(current_ego_pose, ego_info).outer(); + const auto & current_footprint = common_data_ptr->transient_data.current_footprint.outer(); auto ego_min_dist_to_end = std::numeric_limits::max(); - for (const auto & ego_edge_point : ego_polygon) { + for (const auto & ego_edge_point : current_footprint) { const auto ego_edge = autoware::universe_utils::createPoint(ego_edge_point.x(), ego_edge_point.y(), 0.0); const auto dist_to_end = autoware::motion_utils::calcSignedArcLength( @@ -1179,9 +1002,8 @@ bool is_ahead_of_ego( ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); auto current_min_dist_to_end = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path.points, obj_p, path.points.back().point.pose.position); @@ -1192,7 +1014,7 @@ bool is_ahead_of_ego( bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; const auto & lanes_ptr = common_data_ptr->lanes_ptr; @@ -1201,7 +1023,7 @@ bool is_before_terminal( : path.points.back().point.pose.position; double current_max_dist = std::numeric_limits::lowest(); - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & obj_position = object.initial_pose.position; const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength(path.points, obj_position, terminal_position); // we don't always have to check the distance accurately. @@ -1209,8 +1031,7 @@ bool is_before_terminal( return dist_to_base_link >= 0.0; } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_obj_to_terminal = autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); @@ -1230,22 +1051,6 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); } -ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects) -{ - ExtendedPredictedObjects extended_objects; - extended_objects.reserve(objects.size()); - - const auto & bpp_param = *common_data_ptr->bpp_param_ptr; - const auto & lc_param = *common_data_ptr->lc_param_ptr; - std::transform( - objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) { - return utils::lane_change::transform(object, bpp_param, lc_param); - }); - - return extended_objects; -} - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk, const bool ignore_intersection) @@ -1272,48 +1077,221 @@ double get_distance_to_next_regulatory_element( return distance; } -} // namespace autoware::behavior_path_planner::utils::lane_change -namespace autoware::behavior_path_planner::utils::lane_change::debug +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path) { -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) + const auto & path_points = path.points; + auto min_dist_to_obj = std::numeric_limits::max(); + for (const auto & object : filtered_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.linear.x); + if (obj_v > common_data_ptr->lc_param_ptr->th_stop_velocity) { + continue; + } + + // provide "estimation" based on size of object + const auto dist_to_obj = + motion_utils::calcSignedArcLength( + path_points, path_points.front().point.pose.position, object.initial_pose.position) - + (object.shape.dimensions.x / 2); + + if (dist_to_obj < dist_to_target_lane_start) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + for (const auto & polygon_p : object.initial_polygon.outer()) { + const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); + + // ignore if the point is not on ego path + if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { + continue; + } + + const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); + min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + } + } + return min_dist_to_obj; +} + +bool has_blocking_target_object( + const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length, + const PathWithLaneId & path) { - geometry_msgs::msg::Point32 p; - p.x = static_cast(pose.position.x); - p.y = static_cast(pose.position.y); - p.z = static_cast(pose.position.z); - return p; -}; - -geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset) + return ranges::any_of(target_leading_objects.stopped, [&](const auto & object) { + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, object.initial_pose.position); + const auto width_margin = object.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); +} + +bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr) { - 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; - - // if stationary object, extend forward and backward by the half of lon length - const double forward_lon_offset = base_to_front + additional_lon_offset; - const double backward_lon_offset = -base_to_rear; - const double lat_offset = width / 2.0 + additional_lat_offset; - - const auto p1 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); - geometry_msgs::msg::Polygon polygon; - - polygon.points.push_back(create_point32(p1)); - polygon.points.push_back(create_point32(p2)); - polygon.points.push_back(create_point32(p3)); - polygon.points.push_back(create_point32(p4)); - - return polygon; + const auto & transient_data = common_data_ptr->transient_data; + if (transient_data.in_intersection && transient_data.in_turn_direction_lane) { + return false; + } + + return transient_data.dist_from_prev_intersection > + common_data_ptr->lc_param_ptr->backward_length_from_intersection; } -} // namespace autoware::behavior_path_planner::utils::lane_change::debug +std::vector get_line_string_paths(const ExtendedPredictedObject & object) +{ + const auto to_linestring_2d = [](const auto & predicted_path) -> LineString2d { + LineString2d line_string; + const auto & path = predicted_path.path; + line_string.reserve(path.size()); + for (const auto & path_point : path) { + const auto point = universe_utils::fromMsg(path_point.pose.position).to_2d(); + line_string.push_back(point); + } + + return line_string; + }; + + return object.predicted_paths | ranges::views::transform(to_linestring_2d) | + ranges::to(); +} + +bool has_overtaking_turn_lane_object( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects) +{ + // Note: This situation is only applicable if the ego is in a turn lane. + if (has_passed_intersection_turn_direction(common_data_ptr)) { + return false; + } + + const auto is_object_overlap_with_target = [&](const auto & object) { + // to compensate for perception issue, or if object is from behind ego, and tries to overtake, + // but stop all of sudden + if (!boost::geometry::disjoint( + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->current)) { + return true; + } + + return object_path_overlaps_lanes(object, common_data_ptr->lanes_polygon_ptr->target); + }; + + return std::any_of( + trailing_objects.begin(), trailing_objects.end(), is_object_overlap_with_target); +} + +bool filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects) +{ + using behavior_path_planner::utils::path_safety_checker::filter::is_vehicle; + using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; + const auto & lanes_polygon = *common_data_ptr->lanes_polygon_ptr; + const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->safety.th_stopped_object_velocity; + + const auto is_lateral_far = std::invoke([&]() -> bool { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, object.initial_pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (vehicle_width / 2); + }); + + const auto is_stopped = velocity_filter( + object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); + if (is_lateral_far && before_terminal) { + const auto overlapping_with_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target) || + (!is_stopped && is_vehicle(object.classification) && + object_path_overlaps_lanes(object, lanes_polygon.target)); + + if (overlapping_with_target_lanes) { + if (!ahead_of_ego && !is_stopped) { + trailing_objects.push_back(object); + return true; + } + + if (ahead_of_ego) { + if (is_stopped) { + leading_objects.stopped.push_back(object); + } else { + leading_objects.moving.push_back(object); + } + return true; + } + } + + // Check if the object is positioned outside the lane boundary but still close to its edge. + const auto in_expanded_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.expanded_target); + + if (in_expanded_target_lanes && is_stopped && ahead_of_ego) { + leading_objects.stopped_at_bound.push_back(object); + return true; + } + } + + const auto is_overlap_target_backward = + ranges::any_of(lanes_polygon.preceding_target, [&](const auto & target_backward_polygon) { + return !boost::geometry::disjoint(object.initial_polygon, target_backward_polygon); + }); + + // check if the object intersects with target backward lanes + if (is_overlap_target_backward && !is_stopped) { + trailing_objects.push_back(object); + return true; + } + + return false; +} + +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & ego_pose = common_data_ptr->get_ego_pose(); + const auto backward_lane_length = common_data_ptr->lc_param_ptr->backward_lane_length; + + const auto preceding_lanes_list = + utils::getPrecedingLanelets(*route_handler_ptr, target_lanes, ego_pose, backward_lane_length); + + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + std::unordered_set current_lanes_id; + for (const auto & lane : current_lanes) { + current_lanes_id.insert(lane.id()); + } + const auto is_overlapping = [&](const lanelet::ConstLanelet & lane) { + return current_lanes_id.find(lane.id()) != current_lanes_id.end(); + }; + + std::vector non_overlapping_lanes_vec; + for (const auto & lanes : preceding_lanes_list) { + auto lanes_reversed = lanes | ranges::views::reverse; + auto overlapped_itr = ranges::find_if(lanes_reversed, is_overlapping); + + if (overlapped_itr == lanes_reversed.begin()) { + continue; + } + + // Lanes are not reversed by default. Avoid returning reversed lanes to prevent undefined + // behavior. + lanelet::ConstLanelets non_overlapping_lanes(overlapped_itr.base(), lanes.end()); + non_overlapping_lanes_vec.push_back(non_overlapping_lanes); + } + + return non_overlapping_lanes_vec; +} + +bool object_path_overlaps_lanes( + const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) +{ + return ranges::any_of(get_line_string_paths(object), [&](const auto & path) { + return !boost::geometry::disjoint(path, lanes_polygon); + }); +} +} // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp new file mode 100644 index 0000000000000..5ca308d0d4abb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -0,0 +1,229 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_lane_change_module/manager.hpp" +#include "autoware/behavior_path_lane_change_module/scene.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" + +#include + +#include + +#include + +using autoware::behavior_path_planner::LaneChangeModuleManager; +using autoware::behavior_path_planner::LaneChangeModuleType; +using autoware::behavior_path_planner::NormalLaneChange; +using autoware::behavior_path_planner::PlannerData; +using autoware::behavior_path_planner::lane_change::CommonDataPtr; +using autoware::behavior_path_planner::lane_change::LCParamPtr; +using autoware::behavior_path_planner::lane_change::RouteHandlerPtr; +using autoware::route_handler::Direction; +using autoware::route_handler::RouteHandler; +using autoware::test_utils::get_absolute_path_to_config; +using autoware::test_utils::get_absolute_path_to_lanelet_map; +using autoware::test_utils::get_absolute_path_to_route; +using autoware_map_msgs::msg::LaneletMapBin; +using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; + +class TestNormalLaneChange : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + init_param(); + init_module(); + } + + void init_param() + { + auto node_options = get_node_options(); + auto node = rclcpp::Node(name_, node_options); + planner_data_->init_parameters(node); + lc_param_ptr_ = LaneChangeModuleManager::set_params(&node, node.get_name()); + planner_data_->route_handler = init_route_handler(); + + ego_pose_ = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); + planner_data_->self_odometry = set_odometry(ego_pose_); + planner_data_->dynamic_object = + std::make_shared(); + } + + void init_module() + { + normal_lane_change_ = + std::make_shared(lc_param_ptr_, lc_type_, lc_direction_); + normal_lane_change_->setData(planner_data_); + set_previous_approved_path(); + } + + [[nodiscard]] const CommonDataPtr & get_common_data_ptr() const + { + return normal_lane_change_->common_data_ptr_; + } + + [[nodiscard]] rclcpp::NodeOptions get_node_options() const + { + auto node_options = rclcpp::NodeOptions{}; + + const auto common_param = + get_absolute_path_to_config(test_utils_dir_, "test_common.param.yaml"); + const auto nearest_search_param = + get_absolute_path_to_config(test_utils_dir_, "test_nearest_search.param.yaml"); + const auto vehicle_info_param = + get_absolute_path_to_config(test_utils_dir_, "test_vehicle_info.param.yaml"); + + std::string bpp_dir{"autoware_behavior_path_planner"}; + const auto bpp_param = get_absolute_path_to_config(bpp_dir, "behavior_path_planner.param.yaml"); + const auto drivable_area_expansion_param = + get_absolute_path_to_config(bpp_dir, "drivable_area_expansion.param.yaml"); + const auto scene_module_manager_param = + get_absolute_path_to_config(bpp_dir, "scene_module_manager.param.yaml"); + + std::string lc_dir{"autoware_behavior_path_lane_change_module"}; + const auto lc_param = get_absolute_path_to_config(lc_dir, "lane_change.param.yaml"); + + autoware::test_utils::updateNodeOptions( + node_options, {common_param, nearest_search_param, vehicle_info_param, bpp_param, + drivable_area_expansion_param, scene_module_manager_param, lc_param}); + return node_options; + } + + [[nodiscard]] RouteHandlerPtr init_route_handler() const + { + std::string autoware_route_handler_dir{"autoware_route_handler"}; + std::string lane_change_right_test_route_filename{"lane_change_test_route.yaml"}; + std::string lanelet_map_filename{"2km_test.osm"}; + const auto lanelet2_path = + get_absolute_path_to_lanelet_map(test_utils_dir_, lanelet_map_filename); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet2_path, 5.0); + auto route_handler_ptr = std::make_shared(map_bin_msg); + const auto rh_test_route = + get_absolute_path_to_route(autoware_route_handler_dir, lane_change_right_test_route_filename); + route_handler_ptr->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); + + return route_handler_ptr; + } + + static std::shared_ptr set_odometry(const Pose & pose) + { + nav_msgs::msg::Odometry odom; + odom.pose.pose = pose; + return std::make_shared(odom); + } + + void set_previous_approved_path() + { + normal_lane_change_->prev_module_output_.path = create_previous_approved_path(); + } + + [[nodiscard]] PathWithLaneId create_previous_approved_path() const + { + const auto & common_data_ptr = get_common_data_ptr(); + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + lanelet::ConstLanelet closest_lane; + const auto current_pose = planner_data_->self_odometry->pose.pose; + route_handler_ptr->getClosestLaneletWithinRoute(current_pose, &closest_lane); + const auto backward_distance = common_data_ptr->bpp_param_ptr->backward_path_length; + const auto forward_distance = common_data_ptr->bpp_param_ptr->forward_path_length; + const auto current_lanes = route_handler_ptr->getLaneletSequence( + closest_lane, current_pose, backward_distance, forward_distance); + + return route_handler_ptr->getCenterLinePath( + current_lanes, 0.0, std::numeric_limits::max()); + } + + void TearDown() override + { + normal_lane_change_ = nullptr; + lc_param_ptr_ = nullptr; + planner_data_ = nullptr; + rclcpp::shutdown(); + } + + LCParamPtr lc_param_ptr_; + std::shared_ptr normal_lane_change_; + std::shared_ptr planner_data_ = std::make_shared(); + LaneChangeModuleType lc_type_{LaneChangeModuleType::NORMAL}; + Direction lc_direction_{Direction::RIGHT}; + std::string name_{"test_lane_change_scene"}; + std::string test_utils_dir_{"autoware_test_utils"}; + Pose ego_pose_; +}; + +TEST_F(TestNormalLaneChange, testBaseClassInitialize) +{ + const auto type = normal_lane_change_->getModuleType(); + const auto type_str = normal_lane_change_->getModuleTypeStr(); + + ASSERT_EQ(type, LaneChangeModuleType::NORMAL); + const auto is_type_str = type_str == "NORMAL"; + ASSERT_TRUE(is_type_str); + + ASSERT_EQ(normal_lane_change_->getDirection(), Direction::RIGHT); + + ASSERT_TRUE(get_common_data_ptr()); + + ASSERT_TRUE(get_common_data_ptr()->is_data_available()); + ASSERT_FALSE(get_common_data_ptr()->is_lanes_available()); +} + +TEST_F(TestNormalLaneChange, testUpdateLanes) +{ + constexpr auto is_approved = true; + + normal_lane_change_->update_lanes(is_approved); + + ASSERT_FALSE(get_common_data_ptr()->is_lanes_available()); + + normal_lane_change_->update_lanes(!is_approved); + + ASSERT_TRUE(get_common_data_ptr()->is_lanes_available()); +} + +TEST_F(TestNormalLaneChange, testGetPathWhenInvalid) +{ + constexpr auto is_approved = true; + normal_lane_change_->update_lanes(!is_approved); + normal_lane_change_->update_filtered_objects(); + normal_lane_change_->update_transient_data(!is_approved); + normal_lane_change_->updateLaneChangeStatus(); + const auto & lc_status = normal_lane_change_->getLaneChangeStatus(); + + ASSERT_FALSE(lc_status.is_valid_path); +} + +TEST_F(TestNormalLaneChange, testGetPathWhenValid) +{ + constexpr auto is_approved = true; + ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0); + planner_data_->self_odometry = set_odometry(ego_pose_); + set_previous_approved_path(); + normal_lane_change_->update_lanes(!is_approved); + normal_lane_change_->update_filtered_objects(); + normal_lane_change_->update_transient_data(!is_approved); + const auto lane_change_required = normal_lane_change_->isLaneChangeRequired(); + + ASSERT_TRUE(lane_change_required); + + normal_lane_change_->updateLaneChangeStatus(); + const auto & lc_status = normal_lane_change_->getLaneChangeStatus(); + + ASSERT_TRUE(lc_status.is_valid_path); +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 68547a491324d..147a53672f30a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index ceecb7b02408a..60254c692e5d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -327,6 +327,7 @@ class SceneModuleManagerInterface } } +protected: virtual std::unique_ptr createNewSceneModuleInstance() = 0; rclcpp::Node * node_ = nullptr; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index f73f989174b54..fec2a23bc2652 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -34,14 +34,31 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter using autoware_perception_msgs::msg::PredictedObject; using tier4_planning_msgs::msg::PathPointWithLaneId; -bool velocity_filter( - const PredictedObject & object, double velocity_threshold, double max_velocity); +/** + * @brief Filters object based on velocity. + * + * @param twist The twist of predicted object to filter. + * @param velocity_threshold Lower bound + * @param max_velocity Upper bound + * @return Returns true when the object is within a certain velocity range. + */ +bool velocity_filter(const Twist & object_twist, double velocity_threshold, double max_velocity); bool position_filter( PredictedObject & object, const std::vector & path_points, const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); +/** + * @brief Checks if the object classification represents a vehicle (CAR, TRUCK, BUS, TRAILER, + * MOTORCYCLE). + * + * @param classification The object classification to check. + * @return true If the classification is a vehicle type. + * @return false Otherwise. + */ +bool is_vehicle(const ObjectClassification & classification); + } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 5c1eee92c2a5d..04b33e291d945 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -55,6 +55,8 @@ using geometry_msgs::msg::Vector3; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; +static constexpr double eps = 0.01; + struct PolygonPoint { geometry_msgs::msg::Point point; @@ -104,6 +106,33 @@ FrenetPoint convertToFrenetPoint( std::vector getIds(const lanelet::ConstLanelets & lanelets); +/** + * @brief Converts a Lanelet point to a ROS Pose message. + * + * This function converts a point from a Lanelet map to a ROS geometry_msgs::msg::Pose. + * It sets the position from the point and calculates the orientation (yaw) based on the target + * lane. + * + * @tparam LaneletPointType The type of the input point. + * + * @param[in] src_point The point to convert. + * @param[in] target_lane The lanelet used to determine the orientation. + * + * @return A Pose message with the position and orientation of the point. + */ +template +Pose to_geom_msg_pose(const LaneletPointType & src_point, const lanelet::ConstLanelet & target_lane) +{ + const auto point = lanelet::utils::conversion::toGeomMsgPt(src_point); + const auto yaw = lanelet::utils::getLaneletAngle(target_lane, point); + geometry_msgs::msg::Pose pose; + pose.position = point; + tf2::Quaternion quat; + quat.setRPY(0, 0, yaw); + pose.orientation = tf2::toMsg(quat); + return pose; +} + // distance (arclength) calculation double l2Norm(const Vector3 vector); @@ -241,6 +270,10 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. @@ -249,8 +282,10 @@ std::shared_ptr generateCenterLinePath( PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, const bool left_side); double getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, const double base_link2rear, const Pose & vehicle_pose, const bool left_side); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 25b307ab2cc4d..94c6c393ad96b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -26,11 +26,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { -bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity) +bool velocity_filter(const Twist & object_twist, double velocity_threshold, double max_velocity) { - const auto v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); return (velocity_threshold < v_norm && v_norm < max_velocity); } @@ -53,6 +51,20 @@ bool is_within_circle( std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y); return dist < search_radius; } + +bool is_vehicle(const ObjectClassification & classification) +{ + switch (classification.label) { + case ObjectClassification::CAR: + case ObjectClassification::TRUCK: + case ObjectClassification::BUS: + case ObjectClassification::TRAILER: + case ObjectClassification::MOTORCYCLE: + return true; + default: + return false; + } +} } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -146,7 +158,8 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double velocity_threshold, double max_velocity) { const auto filter = [&](const auto & object) { - return filter::velocity_filter(object, velocity_threshold, max_velocity); + return filter::velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, velocity_threshold, max_velocity); }; auto filtered = objects; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 55a6c6ff39d30..3100c409ecab6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -559,19 +559,24 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin) { - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto base_link2front = common_param.base_link2front; - const auto base_link2rear = common_param.base_link2rear; - const auto vehicle_width = common_param.vehicle_width; + const auto combined_lane = lanelet::utils::combineLaneletsShape(current_lanes); + const auto lane_polygon = combined_lane.polygon2d().basicPolygon(); + return isEgoWithinOriginalLane(lane_polygon, current_pose, common_param, outer_margin); +} + +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin) +{ const auto vehicle_poly = autoware::universe_utils::toFootprint( - current_pose, base_link2front, base_link2rear, vehicle_width); + current_pose, common_param.base_link2front, common_param.base_link2rear, + common_param.vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. for (const auto & p : vehicle_poly.outer()) { // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a // positive value. - const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + const auto dist = boost::geometry::distance(p, lane_polygon); if (dist > std::max(outer_margin, 0.0)) { return false; // out of polygon } @@ -817,25 +822,29 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) return path.points.at(*insert_idx); } +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, bool left_side) +{ + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(position); + const auto & boundary_line_2d = left_side ? lanelet.leftBound2d() : lanelet.rightBound2d(); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + return arc_coordinates.distance; +} + double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose, bool left_side) { lanelet::ConstLanelet closest_lanelet; - lanelet::ArcCoordinates arc_coordinates; + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & boundary_line_2d = left_side - ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - arc_coordinates = lanelet::geometry::toArcCoordinates( - boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "closest shoulder lanelet not found."); + return getSignedDistanceFromLaneBoundary(closest_lanelet, pose.position, left_side); } - return arc_coordinates.distance; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), "closest lanelet not found."); + + return 0.0; } std::optional getSignedDistanceFromBoundary( @@ -973,25 +982,8 @@ double getArcLengthToTargetLanelet( const auto target_center_line = target_lane.centerline().basicLineString(); - Pose front_pose, back_pose; - - { - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.front()); - const double front_yaw = lanelet::utils::getLaneletAngle(target_lane, front_point); - front_pose.position = front_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(tf_quat); - } - - { - const auto back_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.back()); - const double back_yaw = lanelet::utils::getLaneletAngle(target_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, back_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - } + const auto front_pose = to_geom_msg_pose(target_center_line.front(), target_lane); + const auto back_pose = to_geom_msg_pose(target_center_line.back(), target_lane); const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose); const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose); @@ -1212,7 +1204,6 @@ PathWithLaneId setDecelerationVelocity( const auto stop_point_length = autoware::motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; - constexpr double eps{0.01}; if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); } @@ -1264,8 +1255,19 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data) { + if (path.points.empty() || !planner_data) { + return {}; + } + const auto & route_handler = planner_data->route_handler; - const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & self_odometry = planner_data->self_odometry; + + if (!route_handler || !self_odometry) { + return {}; + } + + const auto & current_pose = self_odometry->pose.pose; + const auto & p = planner_data->parameters; std::set lane_ids;