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 b4f848e0809aa..ee371f8592833 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 @@ -787,6 +787,51 @@ Depending on the space configuration around the Ego vehicle, it is possible that Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above. +## Generating Path Using Frenet Planner + +!!! warning + + Generating path using Frenet planner applies only when ego is near terminal start + +If the ego vehicle is far from the terminal, the lane change module defaults to using the [path shifter](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design/). This ensures that the lane change is completed while the target lane remains a neighbor of the current lane. However, this approach may result in high curvature paths near the terminal, potentially causing long vehicles to deviate from the lane. + +To address this, the lane change module provides an option to choose between the path shifter and the [Frenet planner](https://autowarefoundation.github.io/autoware.universe/main/planning/sampling_based_planner/autoware_frenet_planner/). The Frenet planner allows for some flexibility in the lane change endpoint, extending the lane changing end point slightly beyond the current lane's neighbors. + +The following table provides comparisons between the planners + +
+ + + + + + + + + + + + + + + + + +
With Path ShifterWith Frenet Planner
Path shifter result at straight laneletsFrenet planner result at straight lanelets
Path shifter result at branching laneletsFrenet planner result at branching lanelets
Path shifter result at curved laneletsFrenet planner result at curved lanelets
+
+ +!!! note + + The planner can be enabled or disabled using the `frenet.enable` flag. + +!!! note + + Since only a segment of the target lane is used as input to generate the lane change path, the end pose of the lane change segment may not smoothly connect to the target lane centerline. To address this, increase the value of `frenet.th_curvature_smoothing` to improve the smoothness. + +!!! note + + The yaw difference threshold (`frenet.th_yaw_diff`) limits the maximum curvature difference between the end of the prepare segment and the lane change segment. This threshold might prevent the generation of a lane change path when the lane curvature is high. In such cases, you can increase the frenet.th_yaw_diff value. However, note that if the prepare path was initially shifted by other modules, the resultant steering may not be continuous. + ## Aborting a Previously Approved Lane Change Once the lane change path is approved, there are several situations where we may need to abort the maneuver. The abort process is triggered when any of the following conditions is met @@ -1008,6 +1053,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `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 | +| `trajectory.th_prepare_curvature` | [-] | double | If the maximum curvature of the prepare segment exceeds the threshold, the prepare segment is invalid. | 0.03 | | `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 | | `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/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | @@ -1057,6 +1103,18 @@ The following parameters are used to configure terminal lane change path feature | `terminal_path.disable_near_goal` | [-] | bool | Flag to disable terminal path feature if ego is near goal | true | | `terminal_path.stop_at_boundary` | [-] | bool | If true, ego will stop at current lane boundary instead of middle of lane | false | +### Generating Lane Changing Path using Frenet Planner + +!!! warning + + Only applicable when ego is near terminal start + +| Name | Unit | Type | Description | Default value | +| :------------------------------ | ----- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `frenet.enable` | [-] | bool | Flag to enable/disable frenet planner when ego is near terminal start. | true | +| `frenet.th_yaw_diff` | [deg] | double | If the yaw diff between of the prepare segment's end and lane changing segment's start exceed the threshold , the lane changing segment is invalid. | 10.0 | +| `frenet.th_curvature_smoothing` | [-] | double | Filters and appends target path points with curvature below the threshold to candidate path. | 0.1 | + ### Collision checks #### Target Objects 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 91b296f8bd40f..df9491576a4ee 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 @@ -29,6 +29,7 @@ lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 lane_changing_decel_factor: 0.5 + th_prepare_curvature: 0.03 # [/] # delay lane change delay_lane_change: @@ -37,6 +38,12 @@ min_road_shoulder_width: 0.5 # [m] th_parked_vehicle_shift_ratio: 0.6 + # trajectory generation near terminal using frenet planner + frenet: + enable: true + th_yaw_diff: 10.0 # [deg] + th_curvature_smoothing: 0.1 # [/] + # safety check safety_check: allow_loose_check_for_cancel: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png new file mode 100644 index 0000000000000..51f3a2fc46500 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png new file mode 100644 index 0000000000000..4af931348d6ca Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png new file mode 100644 index 0000000000000..f834a64867e6c Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png new file mode 100644 index 0000000000000..22ba8d2d7c47d Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png new file mode 100644 index 0000000000000..9a36642b2d545 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png new file mode 100644 index 0000000000000..7a4e3a453304b Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png differ 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 a8b7a670c881b..c11839f0356c6 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 @@ -140,6 +140,18 @@ class NormalLaneChange : public LaneChangeBase bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; + bool get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + + bool get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; 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 index 9e6b9d229d2f2..c844b6b218d10 100644 --- 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 @@ -269,6 +269,7 @@ struct CommonData LanesPolygonPtr lanes_polygon_ptr; TransientData transient_data; PathWithLaneId current_lanes_path; + PathWithLaneId target_lanes_path; ModuleType lc_type; Direction direction; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index 589f2f20ba258..eb07d2f0d3704 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -39,6 +39,20 @@ struct MetricsDebug double max_lane_changing_length; }; +struct FrenetStateDebug +{ + LaneChangePhaseMetrics prep_metric; + frenet_planner::SamplingParameter sampling_parameter; + double max_lane_changing_length; + + FrenetStateDebug( + LaneChangePhaseMetrics prep_metric, frenet_planner::SamplingParameter sampling_param, + const double max_len) + : prep_metric(prep_metric), sampling_parameter(sampling_param), max_lane_changing_length(max_len) + { + } +}; + struct Debug { std::string module_type; @@ -52,6 +66,7 @@ struct Debug lanelet::ConstLanelets target_lanes; lanelet::ConstLanelets target_backward_lanes; std::vector lane_change_metrics; + std::vector frenet_states; double collision_check_object_debug_lifetime{0.0}; double distance_to_end_of_current_lane{std::numeric_limits::max()}; double distance_to_lane_change_finished{std::numeric_limits::max()}; 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 index b25dbc99189e8..f0adcb1d69b42 100644 --- 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 @@ -113,6 +113,13 @@ struct SafetyParameters CollisionCheckParameters collision_check{}; }; +struct FrenetPlannerParameters +{ + bool enable{true}; + double th_yaw_diff_deg{10.0}; + double th_curvature_smoothing{0.1}; +}; + struct TrajectoryParameters { double max_prepare_duration{4.0}; @@ -124,6 +131,7 @@ struct TrajectoryParameters double th_lane_changing_length_diff{0.5}; double min_lane_changing_velocity{5.6}; double lane_changing_decel_factor{0.5}; + double th_prepare_curvature{0.03}; int lon_acc_sampling_num{10}; int lat_acc_sampling_num{10}; LateralAccelerationMap lat_acc_map{}; @@ -151,6 +159,7 @@ struct Parameters CancelParameters cancel{}; DelayParameters delay{}; TerminalPathParameters terminal_path{}; + FrenetPlannerParameters frenet{}; // lane change parameters double backward_lane_length{200.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 0fa0a9d977a26..0fa2c6cc8dfbc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -19,19 +19,53 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include + #include +#include #include namespace autoware::behavior_path_planner::lane_change { +enum class PathType { ConstantJerk = 0, FrenetPlanner }; + using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; +struct TrajectoryGroup +{ + PathWithLaneId prepare; + PathWithLaneId target_lane_ref_path; + std::vector target_lane_ref_path_dist; + LaneChangePhaseMetrics prepare_metric; + frenet_planner::Trajectory lane_changing; + frenet_planner::FrenetState initial_state; + double max_lane_changing_length{0.0}; + + TrajectoryGroup() = default; + TrajectoryGroup( + PathWithLaneId prepare, PathWithLaneId target_lane_ref_path, + std::vector target_lane_ref_path_dist, LaneChangePhaseMetrics prepare_metric, + frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state, + const double max_lane_changing_length) + : prepare(std::move(prepare)), + target_lane_ref_path(std::move(target_lane_ref_path)), + target_lane_ref_path_dist(std::move(target_lane_ref_path_dist)), + prepare_metric(prepare_metric), + lane_changing(std::move(lane_changing)), + initial_state(initial_state), + max_lane_changing_length(max_lane_changing_length) + { + } +}; + struct Path { + Info info; PathWithLaneId path; ShiftedPath shifted_path; - Info info; + TrajectoryGroup frenet_path; + PathType type = PathType::ConstantJerk; }; struct Status @@ -39,7 +73,6 @@ struct Status 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 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/utils/path.hpp index 77ba8fe68a653..42586e7b1df92 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/utils/path.hpp @@ -11,7 +11,6 @@ // 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__PATH_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ @@ -26,6 +25,7 @@ namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::LaneChangePath; using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::TrajectoryGroup; /** * @brief Generates a prepare segment for a lane change maneuver. @@ -98,5 +98,65 @@ LaneChangePath construct_candidate_path( const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); + +/** + * @brief Generates candidate trajectories in the Frenet frame for a lane change maneuver. + * + * This function computes a set of candidate trajectories for the ego vehicle in the Frenet frame, + * based on the provided metrics, target lane reference path, and preparation segments. It ensures + * that the generated trajectories adhere to specified constraints, such as lane boundaries and + * velocity limits, while optimizing for smoothness and curvature. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient + * information. + * @param prev_module_path The previous module's path used as a base for preparation segments. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * + * @return std::vector A vector of trajectory groups representing + * valid lane change candidates, each containing the prepare segment, target reference path, and + * Frenet trajectory. + */ +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics); + +/** + * @brief Constructs a lane change path candidate based on a Frenet trajectory group. + * + * This function generates a candidate lane change path by converting a Frenet trajectory group + * into a shifted path and combining it with a prepare segment. It validates the path's feasibility + * by ensuring yaw differences are within allowable thresholds and calculates lane change + * information, such as acceleration, velocity, and duration. + * + * @param trajectory_group A Frenet trajectory group containing the prepared path and Frenet + * trajectory data. + * @param common_data_ptr Shared pointer to CommonData providing parameters and constraints for lane + * changes. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the lane IDs in the path. + * + * @return std::optional The constructed candidate lane change path if valid, or + * std::nullopt if the path is not feasible. + * + * @throws std::logic_error If the yaw difference exceeds the threshold, or other critical errors + * occur. + */ +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids); + +/** + * @brief Appends the target lane reference path to the candidate lane change path. + * + * This function extends the lane change candidate path by appending points from the + * target lane reference path beyond the lane change end position. The appending process + * is constrained by a curvature threshold to ensure smooth transitions and avoid sharp turns. + * + * @param frenet_candidate The candidate lane change path to which the target reference path is + * appended. This includes the lane change path and associated Frenet trajectory data. + * @param th_curvature_smoothing A threshold for the allowable curvature during the extension + * process. Points with curvature exceeding this threshold are excluded. + */ +void append_target_ref_to_candidate(LaneChangePath & frenet_candidate, const double th_curvature); } // namespace autoware::behavior_path_planner::utils::lane_change + #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ 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 69362994dbbac..422c392cac462 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 @@ -24,6 +24,8 @@ #include #include +#include +#include #include #include @@ -56,6 +58,7 @@ using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::lane_change::TargetLaneLeadingObjects; +using behavior_path_planner::lane_change::TrajectoryGroup; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -69,9 +72,27 @@ bool is_mandatory_lane_change(const ModuleType lc_type); void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids); +/** + * @brief Replaces the current lane IDs with a sorted set of IDs based on a predefined mapping. + * + * This function checks if the current lane IDs match the previously processed lane IDs. + * If they do, it returns the previously sorted IDs for efficiency. Otherwise, it matches + * the current lane IDs to the appropriate sorted IDs from the provided mapping and updates + * the cached values. + * + * @param current_lane_ids The current lane IDs to be replaced or verified. + * @param sorted_lane_ids A vector of sorted lane ID groups, each representing a predefined + * order of IDs for specific conditions. + * @param prev_lane_ids Reference to the previously processed lane IDs for caching purposes. + * @param prev_sorted_lane_ids Reference to the previously sorted lane IDs for caching purposes. + * + * @return std::vector The sorted lane IDs if a match is found, or the original + * `current_lane_ids` if no match exists. + */ +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids); std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); @@ -432,5 +453,24 @@ std::vector> convert_to_predicted_paths( * @return true if the pose is within the target or target neighbor polygons, false otherwise. */ bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose); + +/** + * @brief Converts a lane change frenet candidate into a predicted path for the ego vehicle. + * + * This function generates a predicted path based on the provided Frenet candidate, + * simulating the vehicle's trajectory during the preparation and lane-changing phases. + * It interpolates poses and velocities over the duration of the prediction, considering + * the ego vehicle's initial conditions and the candidate's trajectory data. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and ego vehicle state. + * @param frenet_candidate A Frenet trajectory group representing the lane change candidate. + * @param deceleration_sampling_num Unused parameter for deceleration sampling count. + * + * @return std::vector The predicted path as a series of stamped poses + * with associated velocities over the prediction time. + */ +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num); } // 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 fad98ecf8f8e1..cf7600556080e 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 @@ -23,6 +23,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_frenet_planner autoware_motion_utils autoware_rtc_interface autoware_universe_utils 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 ec000b8fee97c..06a9d505f90ad 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 @@ -63,6 +63,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); p.trajectory.lane_changing_decel_factor = getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); + p.trajectory.th_prepare_curvature = + getOrDeclareParameter(*node, parameter("trajectory.th_prepare_curvature")); p.trajectory.lon_acc_sampling_num = getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); p.trajectory.lat_acc_sampling_num = @@ -213,6 +215,12 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); + // trajectory generation near terminal using frenet planner + p.frenet.enable = getOrDeclareParameter(*node, parameter("frenet.enable")); + p.frenet.th_yaw_diff_deg = getOrDeclareParameter(*node, parameter("frenet.th_yaw_diff")); + p.frenet.th_curvature_smoothing = + getOrDeclareParameter(*node, parameter("frenet.th_curvature_smoothing")); + // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); @@ -338,6 +346,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.max_longitudinal_acc); updateParam( parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); + updateParam( + parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature); int longitudinal_acc_sampling_num = 0; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { @@ -356,6 +366,14 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.th_lane_changing_length_diff); } + { + const std::string ns = "lane_change.frenet."; + updateParam(parameters, ns + "enable", p->frenet.enable); + updateParam(parameters, ns + "th_yaw_diff", p->frenet.th_yaw_diff_deg); + updateParam( + parameters, ns + "th_curvature_smoothing", p->frenet.th_curvature_smoothing); + } + { const std::string ns = "lane_change.safety_check.lane_expansion."; updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); 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 88f58debb886a..7748795851865 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,7 +28,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -36,8 +39,11 @@ #include #include +#include + #include #include +#include #include #include @@ -104,6 +110,9 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->current_lanes_path = route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->target_lanes_path = + route_handler_ptr->getCenterLinePath(target_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); @@ -232,8 +241,6 @@ void NormalLaneChange::updateLaneChangeStatus() // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - - status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -1124,15 +1131,92 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) } const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - 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); const auto prepare_phase_metrics = get_prepare_metrics(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); + if ( + common_data_ptr_->lc_param_ptr->frenet.enable && + common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return get_path_using_frenet( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); + } + + return get_path_using_path_shifter( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); +} + +bool NormalLaneChange::get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + stop_watch_.tic("frenet_candidates"); + constexpr auto found_safe_path = true; + const auto frenet_candidates = utils::lane_change::generate_frenet_candidates( + common_data_ptr_, prev_module_output_.path, prepare_metrics); + RCLCPP_DEBUG( + logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc("frenet_candidates")); + + candidate_paths.reserve(frenet_candidates.size()); + lane_change_debug_.frenet_states.clear(); + lane_change_debug_.frenet_states.reserve(frenet_candidates.size()); + for (const auto & frenet_candidate : frenet_candidates) { + lane_change_debug_.frenet_states.emplace_back( + frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter, + frenet_candidate.max_lane_changing_length); + + std::optional candidate_path_opt; + try { + candidate_path_opt = + utils::lane_change::get_candidate_path(frenet_candidate, common_data_ptr_, sorted_lane_ids); + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + if (!candidate_path_opt) { + continue; + } + + try { + if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { + RCLCPP_DEBUG( + logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]", + frenet_candidates.size(), stop_watch_.toc("frenet_candidates")); + utils::lane_change::append_target_ref_to_candidate( + *candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing); + candidate_paths.push_back(*candidate_path_opt); + return found_safe_path; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + // appending all paths affect performance + if (candidate_paths.empty()) { + candidate_paths.push_back(*candidate_path_opt); + } + } + + RCLCPP_DEBUG( + logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc("frenet_candidates")); + return !found_safe_path; +} + +bool NormalLaneChange::get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + const auto & target_lanes = get_target_lanes(); candidate_paths.reserve( - prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); + prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; const auto dist_to_next_regulatory_element = @@ -1151,7 +1235,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; }; - for (const auto & prep_metric : prepare_phase_metrics) { + for (const auto & prep_metric : prepare_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(), @@ -1195,7 +1279,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { - debug_metrics.lc_metrics.push_back({lc_metric, -1}); + debug_metrics.lc_metrics.emplace_back(lc_metric, -1); const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( @@ -1218,7 +1302,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) } candidate_paths.push_back(candidate_path); - debug_metrics.lc_metrics.back().second = candidate_paths.size() - 1; + debug_metrics.lc_metrics.back().second = static_cast(candidate_paths.size()) - 1; try { if (check_candidate_path_safety(candidate_path, target_objects)) { 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 ef899cceea0d4..bf0af0d133dc7 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 @@ -73,6 +73,34 @@ MarkerArray showAllValidLaneChangePath( marker.points.push_back(point.point.pose.position); } + const auto & info = lc_path.info; + auto text_marker = createDefaultMarker( + "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.1, 0.1, 0.8), colors::yellow()); + const auto prep_idx = points.size() / 4; + text_marker.pose = points.at(prep_idx).point.pose; + text_marker.pose.position.z += 2.0; + text_marker.text = fmt::format( + "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | t: {time:.3f}[s] | L: {length:.3f}[m]", + fmt::arg("vel", info.velocity.prepare), + fmt::arg("lon_acc", info.longitudinal_acceleration.prepare), + fmt::arg("time", info.duration.prepare), fmt::arg("length", info.length.prepare)); + marker_array.markers.push_back(text_marker); + + const auto lc_idx = points.size() / 2; + text_marker.id = ++id; + text_marker.pose = points.at(lc_idx).point.pose; + text_marker.text = fmt::format( + "type: {type} | vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: " + "{lat_acc:.3f}[m/s2] | t: " + "{time:.3f}[s] | L: {length:.3f}[m]", + fmt::arg("type", magic_enum::enum_name(lc_path.type)), + fmt::arg("vel", info.velocity.lane_changing), + fmt::arg("lon_acc", info.longitudinal_acceleration.lane_changing), + fmt::arg("lat_acc", info.lateral_acceleration), fmt::arg("time", info.duration.lane_changing), + fmt::arg("length", info.length.lane_changing)); + marker_array.markers.push_back(text_marker); + marker_array.markers.push_back(marker); } return marker_array; @@ -171,43 +199,78 @@ MarkerArray ShowLaneChangeMetricsInfo( const Debug & debug_data, const geometry_msgs::msg::Pose & pose) { MarkerArray marker_array; - if (debug_data.lane_change_metrics.empty()) { - return marker_array; - } auto text_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); - text_marker.text = - fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + - fmt::format("{:^18}|", "lon_accel[m/s2]") + fmt::format("{:^17}|", "velocity[m/s]") + - fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + - fmt::format("{:^20}|", "max_length_th[m]") + fmt::format("{:^15}\n", "path_index"); - for (const auto & metrics : debug_data.lane_change_metrics) { - text_marker.text += fmt::format("{:-<190}\n", ""); - const auto & p_m = metrics.prep_metric; - text_marker.text += - fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + - fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + - fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + - fmt::format("{:^17.3f}", metrics.max_prepare_length) + fmt::format("{:^15}\n", "-"); - text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); - for (const auto & lc_m : metrics.lc_metrics) { - const auto & metric = lc_m.first; - const auto path_index = lc_m.second < 0 ? "-" : std::to_string(lc_m.second); - text_marker.text += fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", metric.lat_accel) + - fmt::format("{:^21.3f}", metric.actual_lon_accel) + - fmt::format("{:^12.3f}", metric.velocity) + - fmt::format("{:^15.3f}", metric.duration) + - fmt::format("{:^15.3f}", metric.length) + - fmt::format("{:^17.3f}", metrics.max_lane_changing_length) + - fmt::format("{:^15}\n", path_index); + if (!debug_data.lane_change_metrics.empty()) { + text_marker.text = + fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + + fmt::format("{:^18}|", "lon_accel[m/s2]") + fmt::format("{:^17}|", "velocity[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^20}|", "max_length_th[m]") + fmt::format("{:^15}\n", "path_index"); + for (const auto & metrics : debug_data.lane_change_metrics) { + text_marker.text += fmt::format("{:-<190}\n", ""); + const auto & p_m = metrics.prep_metric; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + + fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + + fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + + fmt::format("{:^17.3f}", metrics.max_prepare_length) + fmt::format("{:^15}\n", "-"); + text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); + for (const auto & lc_m : metrics.lc_metrics) { + const auto & metric = lc_m.first; + const auto path_index = lc_m.second < 0 ? "-" : std::to_string(lc_m.second); + text_marker.text += fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", metric.lat_accel) + + fmt::format("{:^21.3f}", metric.actual_lon_accel) + + fmt::format("{:^12.3f}", metric.velocity) + + fmt::format("{:^15.3f}", metric.duration) + + fmt::format("{:^15.3f}", metric.length) + + fmt::format("{:^17.3f}", metrics.max_lane_changing_length) + + fmt::format("{:^15}\n", path_index); + } } + marker_array.markers.push_back(text_marker); + } + + if (!debug_data.frenet_states.empty()) { + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "lon_vel[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^17}|", "lat_accel[m/s2]") + + fmt::format("{:^15}|", "lat_vel[m/s2]") + fmt::format("{:^15}|", "s[m]") + + fmt::format("{:^15}|", "d[m]") + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.frenet_states) { + text_marker.text += fmt::format("{:-<250}\n", ""); + const auto & p_m = metrics.prep_metric; + const auto max_len = metrics.max_lane_changing_length; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^13.3f}", p_m.actual_lon_accel) + + fmt::format("{:^15.3f}", p_m.velocity) + fmt::format("{:^15.3f}", p_m.duration) + + fmt::format("{:^12.3f}", p_m.length) + + fmt::format("{:^13}", "") + // Empty string for lat_accel + fmt::format("{:^13}", "") + // Empty string for lat_vel + fmt::format("{:^15}", "") + // Empty string for s + fmt::format("{:^15}", "") + // Empty string for d // Empty string for d + fmt::format("{:^20.3f}\n", max_len); // Empty string for max_length_t + const auto & lc_m = metrics.sampling_parameter.target_state; // Assuming lc_metric exists + const auto duration = metrics.sampling_parameter.target_duration; + text_marker.text += + fmt::format("{:<17}", "frenet_state:") + + fmt::format("{:^15.3f}", lc_m.longitudinal_acceleration) + + fmt::format("{:^13.3f}", lc_m.longitudinal_velocity) + fmt::format("{:^17.3f}", duration) + + fmt::format("{:^10.3f}", lc_m.position.s) + + fmt::format("{:^19.3f}", lc_m.lateral_acceleration) + + fmt::format("{:^10.3f}", lc_m.lateral_velocity) + + fmt::format("{:^18.3f}", lc_m.position.s) + fmt::format("{:^15.3f}", lc_m.position.d) + + fmt::format("{:^16.3f}\n", max_len); // Empty string for max_length_t + } + + marker_array.markers.push_back(text_marker); } - marker_array.markers.push_back(text_marker); return marker_array; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp index 44ee1624f0f51..df45da3f08fa1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -15,17 +15,22 @@ #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/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include #include +#include #include +#include #include #include #include +#include + #include #include #include @@ -42,8 +47,22 @@ using autoware::behavior_path_planner::lane_change::LCParamPtr; using autoware::behavior_path_planner::LaneChangePhaseMetrics; using autoware::behavior_path_planner::ShiftLine; +using autoware::behavior_path_planner::lane_change::TrajectoryGroup; +using autoware::frenet_planner::FrenetState; +using autoware::frenet_planner::SamplingParameters; +using autoware::route_handler::Direction; +using autoware::sampler_common::FrenetPoint; +using autoware::sampler_common::transform::Spline2D; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using geometry_msgs::msg::Pose; +template +T sign(const Direction direction) +{ + return static_cast(direction == Direction::LEFT ? 1.0 : -1.0); +} + double calc_resample_interval( const double lane_changing_length, const double lane_changing_velocity) { @@ -53,7 +72,7 @@ double calc_resample_interval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -PathWithLaneId get_reference_path_from_target_Lane( +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) { @@ -146,11 +165,197 @@ std::optional exceed_yaw_threshold( } return std::nullopt; } + +Spline2D init_reference_spline(const std::vector & target_lanes_ref_path) +{ + std::vector xs; + std::vector ys; + xs.reserve(target_lanes_ref_path.size()); + ys.reserve(target_lanes_ref_path.size()); + for (const auto & p : target_lanes_ref_path) { + xs.push_back(p.point.pose.position.x); + ys.push_back(p.point.pose.position.y); + } + + return {xs, ys}; +} + +FrenetState init_frenet_state( + const FrenetPoint & start_position, const LaneChangePhaseMetrics & prepare_metrics) +{ + FrenetState initial_state; + initial_state.position = start_position; + initial_state.longitudinal_velocity = prepare_metrics.velocity; + initial_state.lateral_velocity = + 0.0; // TODO(Maxime): this can be sampled if we want but it would impact the LC duration + initial_state.longitudinal_acceleration = prepare_metrics.sampled_lon_accel; + initial_state.lateral_acceleration = prepare_metrics.lat_accel; + return initial_state; +} + +std::optional init_sampling_parameters( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prepare_metrics, + const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose) +{ + const auto & trajectory = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_vel = trajectory.min_lane_changing_velocity; + const auto [min_lateral_acc, max_lateral_acc] = + trajectory.lat_acc_map.find(std::max(min_lc_vel, prepare_metrics.velocity)); + const auto duration = autoware::motion_utils::calc_shift_time_from_jerk( + std::abs(initial_state.position.d), trajectory.lateral_jerk, max_lateral_acc); + const auto final_velocity = + std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration); + const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5; + const auto target_s = initial_state.position.s + lc_length; + const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation); + const auto target_lat_vel = + (1.0 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) * + std::tan(initial_yaw - ref_spline.yaw(target_s)); + + if (std::isnan(target_lat_vel)) { + return std::nullopt; + } + + SamplingParameters sampling_parameters; + const auto & safety = common_data_ptr->lc_param_ptr->safety; + sampling_parameters.resolution = safety.collision_check.prediction_time_resolution; + sampling_parameters.parameters.emplace_back(); + sampling_parameters.parameters.back().target_duration = duration; + sampling_parameters.parameters.back().target_state.position = {target_s, 0.0}; + // TODO(Maxime): not sure if we should use curvature at initial or target s + sampling_parameters.parameters.back().target_state.lateral_velocity = + sign(common_data_ptr->direction) * target_lat_vel; + sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0; + sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity; + sampling_parameters.parameters.back().target_state.longitudinal_acceleration = + prepare_metrics.sampled_lon_accel; + return sampling_parameters; +} + +std::vector calc_curvatures( + const std::vector & points, const Pose & current_pose) +{ + const auto nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(points, current_pose.position); + + // Ignore all path points behind ego vehicle. + if (points.size() <= nearest_segment_idx + 2) { + return {}; + } + + std::vector curvatures; + curvatures.reserve(points.size() - nearest_segment_idx + 2); + for (const auto & [p1, p2, p3] : ranges::views::zip( + points | ranges::views::drop(nearest_segment_idx), + points | ranges::views::drop(nearest_segment_idx + 1), + points | ranges::views::drop(nearest_segment_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(p1); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + + curvatures.push_back(autoware::universe_utils::calcCurvature(point1, point2, point3)); + } + + return curvatures; +} + +double calc_average_curvature(const std::vector & curvatures) +{ + const auto filter_zeros = [](const auto & k) { return k != 0.0; }; + auto filtered_k = curvatures | ranges::views::filter(filter_zeros); + if (filtered_k.empty()) { + return 0.0; + } + const auto sums_of_curvatures = [](float sum, const double k) { return sum + std::abs(k); }; + const auto sum_of_k = ranges::accumulate(filtered_k, 0.0, sums_of_curvatures); + const auto count_k = static_cast(ranges::distance(filtered_k)); + return sum_of_k / count_k; +} + +LineString2d get_linestring_bound(const lanelet::ConstLanelets & lanes, const Direction direction) +{ + LineString2d line_string; + const auto & get_bound = [&](const lanelet::ConstLanelet & lane) { + return (direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d(); + }; + + const auto reserve_size = ranges::accumulate( + lanes, 0UL, + [&](auto sum, const lanelet::ConstLanelet & lane) { return sum + get_bound(lane).size(); }); + line_string.reserve(reserve_size); + for (const auto & lane : lanes) { + ranges::for_each(get_bound(lane), [&line_string](const auto & point) { + boost::geometry::append(line_string, Point2d(point.x(), point.y())); + }); + } + return line_string; +} + +Point2d shift_point(const Point2d & p1, const Point2d & p2, const double offset) +{ + // Calculate the perpendicular vector + double dx = p2.x() - p1.x(); + double dy = p2.y() - p1.y(); + double length = std::sqrt(dx * dx + dy * dy); + + // Normalize and find the perpendicular direction + double nx = -dy / length; + double ny = dx / length; + + return {p1.x() + nx * offset, p1.y() + ny * offset}; +} + +bool check_out_of_bound_paths( + const CommonDataPtr & common_data_ptr, const std::vector & lane_changing_poses, + const LineString2d & lane_boundary, const Direction direction) +{ + const auto distance = (0.5 * common_data_ptr->bpp_param_ptr->vehicle_width + 0.1); + const auto offset = sign(direction) * distance; // invert direction + if (lane_changing_poses.size() <= 2) { + return true; // Remove candidates with insufficient poses + } + + LineString2d path_ls; + path_ls.reserve(lane_changing_poses.size()); + + const auto segments = lane_changing_poses | ranges::views::sliding(2); + ranges::for_each(segments | ranges::views::drop(1), [&](const auto & segment) { + const auto & p1 = segment[0].position; + const auto & p2 = segment[1].position; + boost::geometry::append(path_ls, shift_point({p2.x, p2.y}, {p1.x, p1.y}, offset)); + }); + + return boost::geometry::disjoint(path_ls, lane_boundary); // Remove if disjoint +} + +double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose) +{ + const auto dist_to_target_end = std::invoke([&]() { + if (common_data_ptr->lanes_ptr->target_lane_in_goal_section) { + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->route_handler_ptr->getGoalPose().position); + } + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->target_lanes_path.points.back().point.pose.position); + }); + + // v2 = u2 + 2ad + // u = sqrt(2ad) + return std::clamp( + std::sqrt( + std::abs(2.0 * common_data_ptr->bpp_param_ptr->min_acc * std::max(dist_to_target_end, 0.0))), + common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity, + common_data_ptr->bpp_param_ptr->max_vel); +} + }; // namespace namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::PathType; bool get_prepare_segment( const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, @@ -187,7 +392,17 @@ bool get_prepare_segment( throw std::logic_error("lane change start is behind target lanelet!"); } - return true; + const auto curvatures = calc_curvatures(prepare_segment.points, common_data_ptr->get_ego_pose()); + + // curvatures may be empty if ego is near the terminal start of the path. + if (curvatures.empty()) { + return true; + } + + const auto average_curvature = calc_average_curvature(curvatures); + + RCLCPP_DEBUG(get_logger(), "average curvature: %.3f", average_curvature); + return average_curvature <= common_data_ptr->lc_param_ptr->trajectory.th_prepare_curvature; } LaneChangePath get_candidate_path( @@ -205,7 +420,7 @@ LaneChangePath get_candidate_path( } const auto & lc_start_pose = prep_segment.points.back().point.pose; - const auto target_lane_reference_path = get_reference_path_from_target_Lane( + const auto target_lane_reference_path = get_reference_path_from_target_lane( common_data_ptr, lc_start_pose, lc_metric.length, resample_interval); if (target_lane_reference_path.points.empty()) { @@ -259,10 +474,14 @@ LaneChangePath construct_candidate_path( throw std::logic_error("Lane change end idx not found on target path."); } + std::vector prev_ids; + std::vector prev_sorted_ids; for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); if (i < *lc_end_idx_opt) { - point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + const auto & current_ids = point.lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); continue; @@ -286,7 +505,251 @@ LaneChangePath construct_candidate_path( candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; candidate_path.info = lane_change_info; + candidate_path.type = PathType::ConstantJerk; return candidate_path; } + +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics) +{ + std::vector trajectory_groups; + universe_utils::StopWatch sw; + + const auto & transient_data = common_data_ptr->transient_data; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto direction = common_data_ptr->direction; + const auto current_lane_boundary = get_linestring_bound(current_lanes, direction); + + for (const auto & metric : prep_metrics) { + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr, prev_module_path, metric.length, prepare_segment)) { + RCLCPP_DEBUG(get_logger(), "Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), "%s", e.what()); + break; + } + const auto lc_start_pose = prepare_segment.points.back().point.pose; + + const auto dist_to_end_from_lc_start = + calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr, target_lanes, lc_start_pose) - + common_data_ptr->lc_param_ptr->lane_change_finish_judge_buffer; + const auto max_lc_len = transient_data.lane_changing_length.max; + const auto max_lane_changing_length = std::min(dist_to_end_from_lc_start, max_lc_len); + + constexpr auto resample_interval = 0.5; + const auto target_lane_reference_path = get_reference_path_from_target_lane( + common_data_ptr, lc_start_pose, max_lane_changing_length, resample_interval); + if (target_lane_reference_path.points.empty()) { + continue; + } + + std::vector target_ref_path_sums{0.0}; + target_ref_path_sums.reserve(target_lane_reference_path.points.size() - 1); + double ref_s = 0.0; + for (auto it = target_lane_reference_path.points.begin(); + std::next(it) != target_lane_reference_path.points.end(); ++it) { + ref_s += universe_utils::calcDistance2d(*it, *std::next(it)); + target_ref_path_sums.push_back(ref_s); + } + + const auto reference_spline = init_reference_spline(target_lane_reference_path.points); + + const auto initial_state = init_frenet_state( + reference_spline.frenet({lc_start_pose.position.x, lc_start_pose.position.y}), metric); + + RCLCPP_DEBUG( + get_logger(), "Initial state [s=%2.2f, d=%2.2f, s'=%2.2f, d'=%2.2f, s''=%2.2f, d''=%2.2f]", + initial_state.position.s, initial_state.position.d, initial_state.longitudinal_velocity, + initial_state.lateral_velocity, initial_state.longitudinal_acceleration, + initial_state.lateral_acceleration); + + const auto sampling_parameters_opt = init_sampling_parameters( + common_data_ptr, metric, initial_state, reference_spline, lc_start_pose); + + if (!sampling_parameters_opt) { + continue; + } + + auto frenet_trajectories = frenet_planner::generateTrajectories( + reference_spline, initial_state, *sampling_parameters_opt); + + trajectory_groups.reserve(trajectory_groups.size() + frenet_trajectories.size()); + for (const auto & traj : frenet_trajectories) { + if (!trajectory_groups.empty()) { + const auto diff = std::abs( + traj.frenet_points.back().s - + trajectory_groups.back().lane_changing.frenet_points.back().s); + if (diff < common_data_ptr->lc_param_ptr->trajectory.th_lane_changing_length_diff) { + continue; + } + } + + const auto out_of_bound = + check_out_of_bound_paths(common_data_ptr, traj.poses, current_lane_boundary, direction); + + if (out_of_bound) { + continue; + } + + trajectory_groups.emplace_back( + prepare_segment, target_lane_reference_path, target_ref_path_sums, metric, traj, + initial_state, max_lane_changing_length); + } + } + + const auto limit_vel = [&](TrajectoryGroup & group) { + const auto max_vel = calc_limit(common_data_ptr, group.lane_changing.poses.back()); + for (auto & vel : group.lane_changing.longitudinal_velocities) { + vel = std::clamp(vel, 0.0, max_vel); + } + }; + + ranges::for_each(trajectory_groups, limit_vel); + + ranges::sort(trajectory_groups, [&](const auto & p1, const auto & p2) { + return calc_average_curvature(p1.lane_changing.curvatures) < + calc_average_curvature(p2.lane_changing.curvatures); + }); + + return trajectory_groups; +} + +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids) +{ + if (trajectory_group.lane_changing.frenet_points.empty()) { + return std::nullopt; + } + + ShiftedPath shifted_path; + std::vector prev_ids; + std::vector prev_sorted_ids; + const auto & lane_changing_candidate = trajectory_group.lane_changing; + const auto & target_lane_ref_path = trajectory_group.target_lane_ref_path; + const auto & prepare_segment = trajectory_group.prepare; + const auto & prepare_metric = trajectory_group.prepare_metric; + const auto & initial_state = trajectory_group.initial_state; + const auto & target_ref_sums = trajectory_group.target_lane_ref_path_dist; + auto zipped_candidates = ranges::views::zip( + lane_changing_candidate.poses, lane_changing_candidate.frenet_points, + lane_changing_candidate.longitudinal_velocities, lane_changing_candidate.lateral_velocities, + lane_changing_candidate.curvatures); + + shifted_path.path.points.reserve(zipped_candidates.size()); + shifted_path.shift_length.reserve(zipped_candidates.size()); + for (const auto & [pose, frenet_point, longitudinal_velocity, lateral_velocity, curvature] : + zipped_candidates) { + // Find the reference index + const auto & s = frenet_point.s; + auto ref_i_itr = std::find_if( + target_ref_sums.begin(), target_ref_sums.end(), + [s](const double ref_s) { return ref_s > s; }); + auto ref_i = std::distance(target_ref_sums.begin(), ref_i_itr); + + PathPointWithLaneId point; + point.point.pose = pose; + point.point.longitudinal_velocity_mps = static_cast(longitudinal_velocity); + point.point.lateral_velocity_mps = static_cast(lateral_velocity); + point.point.heading_rate_rps = static_cast(curvature); + point.point.pose.position.z = target_lane_ref_path.points[ref_i].point.pose.position.z; + const auto & current_ids = target_lane_ref_path.points[ref_i].lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); + + // Add to shifted path + shifted_path.shift_length.push_back(frenet_point.d); + shifted_path.path.points.push_back(point); + } + + if (shifted_path.path.points.empty()) { + return std::nullopt; + } + + for (auto & point : shifted_path.path.points) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast(shifted_path.path.points.back().point.longitudinal_velocity_mps)); + } + + const auto th_yaw_diff_deg = common_data_ptr->lc_param_ptr->frenet.th_yaw_diff_deg; + if ( + const auto yaw_diff_opt = exceed_yaw_threshold( + prepare_segment, shifted_path.path, autoware::universe_utils::deg2rad(th_yaw_diff_deg))) { + const auto yaw_diff_deg = autoware::universe_utils::rad2deg(yaw_diff_opt.value()); + const auto err_msg = fmt::format( + "Excessive yaw difference {yaw_diff:2.2f}[deg]. The threshold is {th_yaw_diff:2.2f}[deg].", + fmt::arg("yaw_diff", yaw_diff_deg), fmt::arg("th_yaw_diff", th_yaw_diff_deg)); + throw std::logic_error(err_msg); + } + + LaneChangeInfo info; + info.longitudinal_acceleration = { + prepare_metric.actual_lon_accel, lane_changing_candidate.longitudinal_accelerations.front()}; + info.velocity = {prepare_metric.velocity, initial_state.longitudinal_velocity}; + info.duration = { + prepare_metric.duration, lane_changing_candidate.sampling_parameter.target_duration}; + info.length = {prepare_metric.length, lane_changing_candidate.lengths.back()}; + info.lane_changing_start = prepare_segment.points.back().point.pose; + info.lane_changing_end = lane_changing_candidate.poses.back(); + + ShiftLine sl; + + sl.start = lane_changing_candidate.poses.front(); + sl.end = lane_changing_candidate.poses.back(); + sl.start_shift_length = 0.0; + sl.end_shift_length = initial_state.position.d; + sl.start_idx = 0UL; + sl.end_idx = shifted_path.shift_length.size() - 1; + + info.shift_line = sl; + info.terminal_lane_changing_velocity = lane_changing_candidate.longitudinal_velocities.back(); + info.lateral_acceleration = lane_changing_candidate.lateral_accelerations.front(); + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.info = info; + candidate_path.shifted_path = shifted_path; + candidate_path.frenet_path = trajectory_group; + candidate_path.type = PathType::FrenetPlanner; + + return candidate_path; +} + +void append_target_ref_to_candidate( + LaneChangePath & frenet_candidate, const double th_curvature_smoothing) +{ + const auto & target_lane_ref_path = frenet_candidate.frenet_path.target_lane_ref_path.points; + const auto & lc_end_pose = frenet_candidate.info.lane_changing_end; + const auto lc_end_idx = + motion_utils::findNearestIndex(target_lane_ref_path, lc_end_pose.position); + auto & candidate_path = frenet_candidate.path.points; + if (target_lane_ref_path.size() <= lc_end_idx + 2) { + return; + } + const auto add_size = target_lane_ref_path.size() - (lc_end_idx + 1); + candidate_path.reserve(candidate_path.size() + add_size); + const auto & points = target_lane_ref_path; + for (const auto & [p2, p3] : ranges::views::zip( + points | ranges::views::drop(lc_end_idx + 1), + points | ranges::views::drop(lc_end_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(candidate_path.back().point.pose); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + const auto curvature = + std::abs(autoware::universe_utils::calcCurvature(point1, point2, point3)); + if (curvature < th_curvature_smoothing) { + candidate_path.push_back(p2); + } + } + candidate_path.push_back(target_lane_ref_path.back()); +} } // namespace autoware::behavior_path_planner::utils::lane_change 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 9360435891632..9002270185847 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 @@ -24,12 +24,18 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +// for the geometry types +#include +#include +// for the svg mapper #include #include #include #include #include #include +#include +#include #include #include #include @@ -40,9 +46,10 @@ #include #include -#include - +#include #include +#include +#include #include #include @@ -64,9 +71,11 @@ 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; +using behavior_path_planner::lane_change::PathType; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -382,18 +391,26 @@ std::vector> get_sorted_lane_ids(const CommonDataPtr & comm return sorted_lane_ids; } -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids) +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids) { - for (const auto original_id : original_lane_ids) { + if (current_lane_ids == prev_lane_ids) { + return prev_sorted_lane_ids; + } + + for (const auto original_id : current_lane_ids) { for (const auto & sorted_id : sorted_lane_ids) { if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { - return sorted_id; + prev_lane_ids = current_lane_ids; + prev_sorted_lane_ids = sorted_id; + return prev_sorted_lane_ids; } } } - return original_lane_ids; + + return current_lane_ids; } CandidateOutput assignToCandidate( @@ -475,6 +492,7 @@ std::vector convert_to_predicted_path( 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); } @@ -1127,6 +1145,10 @@ std::vector> convert_to_predicted_paths( const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); const auto ego_predicted_path = [&](size_t n) { + if (lane_change_path.type == PathType::FrenetPlanner) { + return convert_to_predicted_path( + common_data_ptr, lane_change_path.frenet_path, deceleration_sampling_num); + } auto acc = lane_changing_acc + static_cast(n) * acc_resolution; return utils::lane_change::convert_to_predicted_path(common_data_ptr, lane_change_path, acc); }; @@ -1135,6 +1157,48 @@ std::vector> convert_to_predicted_paths( ranges::to(); } +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num) +{ + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_time = frenet_candidate.prepare_metric.duration; + const auto resolution = + common_data_ptr->lc_param_ptr->safety.collision_check.prediction_time_resolution; + const auto prepare_acc = frenet_candidate.prepare_metric.sampled_lon_accel; + std::vector predicted_path; + const auto & path = frenet_candidate.prepare.points; + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); + + const auto vehicle_pose_frenet = + convertToFrenetPoint(path, vehicle_pose.position, nearest_seg_idx); + + for (double t = 0.0; t < prepare_time; t += resolution) { + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, frenet_candidate.prepare_metric.velocity); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto pose = + autoware::motion_utils::calcInterpolatedPose(path, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + const auto & poses = frenet_candidate.lane_changing.poses; + const auto & velocities = frenet_candidate.lane_changing.longitudinal_velocities; + const auto & times = frenet_candidate.lane_changing.times; + + for (const auto [t, pose, velocity] : + ranges::views::zip(times, poses, velocities) | ranges::views::drop(1)) { + predicted_path.emplace_back(prepare_time + t, pose, velocity); + } + + return predicted_path; +} + bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) { const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index cab4acfbb8556..54d4bc2e90714 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -197,6 +197,15 @@ void calculateCartesian( trajectory.longitudinal_accelerations.push_back(0.0); trajectory.lateral_accelerations.push_back(0.0); } + for (auto i = 0UL; i < trajectory.points.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = trajectory.points[i].x(); + pose.position.y = trajectory.points[i].y(); + pose.position.z = 0.0; + pose.orientation = + autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, trajectory.yaws[i]); + trajectory.poses.push_back(pose); + } } }