From 7d15fdf6242a3c28c6f9380413cb81ee8c3658c6 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 28 Feb 2023 12:06:05 +0900 Subject: [PATCH 01/24] perf(lane_change): only compute interpolate ego once (#2839) * fix(lane_change): improvement on isLaneChangePathSafe computation Signed-off-by: Muhammad Zulfaqar Azmi * re-add logic to ignore parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi * fix the duration issues and some code editing Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/utilities.hpp | 22 +++--- .../src/util/lane_change/util.cpp | 55 ++++++++------ .../behavior_path_planner/src/utilities.cpp | 76 ++++++++++++------- 3 files changed, 91 insertions(+), 62 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 4f9441bff3d38..4b22fce427764 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/marker_util/debug_utilities.hpp" +#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/util/pull_out/pull_out_path.hpp" #include @@ -493,19 +494,20 @@ bool isLateralDistanceEnough( const double & relative_lateral_distance, const double & lateral_distance_threshold); bool isSafeInLaneletCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const PredictedPath & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_decel, + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug); bool isSafeInFreeSpaceCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double front_decel, const double rear_decel, CollisionCheckDebug & debug); + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, + const double rear_decel, CollisionCheckDebug & debug); bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index f2d72f4659d2e..ea74ffdae881b 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -506,19 +506,16 @@ bool isLaneChangePathSafe( } const double time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & lane_change_prepare_duration = lane_change_path.duration.prepare; - const auto & enable_collision_check_at_prepare_phase = + const auto check_at_prepare_phase = lane_change_parameters.enable_collision_check_at_prepare_phase; - const auto & lane_changing_safety_check_duration = lane_change_path.duration.lane_changing; - const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration; + + const double check_start_time = check_at_prepare_phase ? 0.0 : lane_change_path.duration.prepare; + const double check_end_time = lane_change_path.duration.sum(); const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity}; const auto vehicle_predicted_path = util::convertToPredictedPath( path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, time_resolution, acceleration, min_lc_speed); - const auto prepare_phase_ignore_target_speed_thresh = - lane_change_parameters.prepare_phase_ignore_target_speed_thresh; - const auto & vehicle_info = common_parameters.vehicle_info; auto in_lane_object_indices = dynamic_objects_indices.target_lane; @@ -551,26 +548,42 @@ bool isLaneChangePathSafe( } }; + const auto reserve_size = + static_cast((check_end_time - check_start_time) / time_resolution); + std::vector check_durations{}; + std::vector> interpolated_ego{}; + check_durations.reserve(reserve_size); + interpolated_ego.reserve(reserve_size); + + { + Pose expected_ego_pose = current_pose; + for (double t = check_start_time; t < check_end_time; t += time_resolution) { + std::string failed_reason; + tier4_autoware_utils::Polygon2d ego_polygon; + [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( + current_pose, vehicle_predicted_path, ego_polygon, t, vehicle_info, expected_ego_pose, + failed_reason); + check_durations.push_back(t); + interpolated_ego.emplace_back(expected_ego_pose, ego_polygon); + } + } + for (const auto & i : in_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); - const auto object_speed = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); - const double check_start_time = (enable_collision_check_at_prepare_phase && - (object_speed > prepare_phase_ignore_target_speed_thresh)) - ? 0.0 - : lane_change_prepare_duration; auto current_debug_data = assignDebugData(obj); const auto predicted_paths = util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, - check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, + interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, + obj, obj_path, common_parameters, + lane_change_parameters.prepare_phase_ignore_target_speed_thresh, front_decel, rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } } + appendDebugInfo(current_debug_data, true); } if (!lane_change_parameters.use_predicted_path_outside_lanelet) { @@ -585,16 +598,10 @@ bool isLaneChangePathSafe( const auto predicted_paths = util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); - const auto object_speed = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); - const double check_start_time = (enable_collision_check_at_prepare_phase && - (object_speed > prepare_phase_ignore_target_speed_thresh)) - ? 0.0 - : lane_change_prepare_duration; if (!util::isSafeInFreeSpaceCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, - check_end_time, time_resolution, obj, common_parameters, front_decel, rear_decel, - current_debug_data.second)) { + interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, obj, + common_parameters, lane_change_parameters.prepare_phase_ignore_target_speed_thresh, + front_decel, rear_decel, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index ed8619fe1390a..887e74bf393aa 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2290,29 +2290,38 @@ bool isLateralDistanceEnough( } bool isSafeInLaneletCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const PredictedPath & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_decel, + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug) { - const auto lerp_path_reserve = (check_end_time - check_start_time) / check_time_resolution; - if (lerp_path_reserve > 1e-3) { - debug.lerped_path.reserve(static_cast(lerp_path_reserve)); - } + debug.lerped_path.reserve(check_duration.size()); Pose expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; - Pose expected_ego_pose = ego_current_pose; - for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { + const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + const auto object_speed = l2Norm(object_twist.linear); + const auto ignore_check_at_time = [&](const double current_time) { + return ( + (current_time < prepare_duration) && + (object_speed < prepare_phase_ignore_target_speed_thresh)); + }; + + for (size_t i = 0; i < check_duration.size(); ++i) { + const auto current_time = check_duration.at(i); + + if (ignore_check_at_time(current_time)) { + continue; + } + tier4_autoware_utils::Polygon2d obj_polygon; [[maybe_unused]] const auto get_obj_info = util::getObjectExpectedPoseAndConvertToPolygon( - target_object_path, target_object, obj_polygon, t, expected_obj_pose, debug.failed_reason); - - tier4_autoware_utils::Polygon2d ego_polygon; - [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose, + target_object_path, target_object, obj_polygon, current_time, expected_obj_pose, debug.failed_reason); + const auto & ego_info = interpolated_ego.at(i); + auto expected_ego_pose = ego_info.first; + const auto & ego_polygon = ego_info.second; debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; @@ -2328,7 +2337,6 @@ bool isSafeInLaneletCollisionCheck( debug.expected_ego_pose = expected_ego_pose; debug.expected_obj_pose = expected_obj_pose; - const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; if (!util::hasEnoughDistance( expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, common_parameters, front_decel, rear_decel, debug)) { @@ -2341,22 +2349,34 @@ bool isSafeInLaneletCollisionCheck( } bool isSafeInFreeSpaceCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double front_decel, const double rear_decel, CollisionCheckDebug & debug) + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, + const double rear_decel, CollisionCheckDebug & debug) { tier4_autoware_utils::Polygon2d obj_polygon; if (!util::calcObjectPolygon(target_object, &obj_polygon)) { return false; } - Pose expected_ego_pose = ego_current_pose; - for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { - tier4_autoware_utils::Polygon2d ego_polygon; - [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose, - debug.failed_reason); + + const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + const auto object_speed = l2Norm(object_twist.linear); + const auto ignore_check_at_time = [&](const double current_time) { + return ( + (current_time < prepare_duration) && + (object_speed < prepare_phase_ignore_target_speed_thresh)); + }; + for (size_t i = 0; i < check_duration.size(); ++i) { + const auto current_time = check_duration.at(i); + + if (ignore_check_at_time(current_time)) { + continue; + } + const auto & ego_info = interpolated_ego.at(i); + auto expected_ego_pose = ego_info.first; + const auto & ego_polygon = ego_info.second; debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; From ea58e59b57330672b0dac99e66150d0b7da321b3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 28 Feb 2023 16:05:43 +0900 Subject: [PATCH 02/24] docs(behavior_path_planner): align document title (#2956) * docs(behavior_path_planner): align document title Signed-off-by: satoshi-ota * style(pre-commit): autofix --------- Signed-off-by: satoshi-ota Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- ...d => behavior_path_planner_avoidance_design.md} | 2 +- ... behavior_path_planner_drivable_area_design.md} | 2 +- ...=> behavior_path_planner_lane_change_design.md} | 2 +- ...ons.md => behavior_path_planner_limitations.md} | 2 +- ...ehavior_path_planner_path_generation_design.md} | 14 +++++++------- ... => behavior_path_planner_side_shift_design.md} | 2 +- ...=> behavior_path_planner_turn_signal_design.md} | 2 +- 7 files changed, 13 insertions(+), 13 deletions(-) rename planning/behavior_path_planner/{behavior_path_planner_avoidance-design.md => behavior_path_planner_avoidance_design.md} (99%) rename planning/behavior_path_planner/{behavior_path_planner_drivable_area.md => behavior_path_planner_drivable_area_design.md} (99%) rename planning/behavior_path_planner/{behavior_path_planner_lane_change.md => behavior_path_planner_lane_change_design.md} (99%) rename planning/behavior_path_planner/{behavior_path_planner_design_limitations.md => behavior_path_planner_limitations.md} (99%) rename planning/behavior_path_planner/{behavior_path_planner_path_generation.md => behavior_path_planner_path_generation_design.md} (94%) rename planning/behavior_path_planner/{behavior_path_planner_side-shift.md => behavior_path_planner_side_shift_design.md} (99%) rename planning/behavior_path_planner/{behavior_path_planner_turn_signal-design.md => behavior_path_planner_turn_signal_design.md} (99%) diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance_design.md similarity index 99% rename from planning/behavior_path_planner/behavior_path_planner_avoidance-design.md rename to planning/behavior_path_planner/behavior_path_planner_avoidance_design.md index 1f3ff22093689..d071e9cc49e36 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance_design.md @@ -1,4 +1,4 @@ -# Avoidance Module +# Avoidance design This is a rule-based path planning module designed for obstacle avoidance. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area_design.md similarity index 99% rename from planning/behavior_path_planner/behavior_path_planner_drivable_area.md rename to planning/behavior_path_planner/behavior_path_planner_drivable_area_design.md index 9ad66b43d5900..d14b6d86108da 100644 --- a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area_design.md @@ -1,4 +1,4 @@ -# Drivable Area +# Drivable Area design Drivable Area represents the area where ego vehicle can pass. diff --git a/planning/behavior_path_planner/behavior_path_planner_lane_change.md b/planning/behavior_path_planner/behavior_path_planner_lane_change_design.md similarity index 99% rename from planning/behavior_path_planner/behavior_path_planner_lane_change.md rename to planning/behavior_path_planner/behavior_path_planner_lane_change_design.md index 532383b488767..756dda62e4d95 100644 --- a/planning/behavior_path_planner/behavior_path_planner_lane_change.md +++ b/planning/behavior_path_planner/behavior_path_planner_lane_change_design.md @@ -1,4 +1,4 @@ -# Lane Change +# Lane Change design The Lane Change module is activated when lane change is needed and can be safely executed. diff --git a/planning/behavior_path_planner/behavior_path_planner_design_limitations.md b/planning/behavior_path_planner/behavior_path_planner_limitations.md similarity index 99% rename from planning/behavior_path_planner/behavior_path_planner_design_limitations.md rename to planning/behavior_path_planner/behavior_path_planner_limitations.md index 0d92728c505ab..8e34c8cc64b05 100644 --- a/planning/behavior_path_planner/behavior_path_planner_design_limitations.md +++ b/planning/behavior_path_planner/behavior_path_planner_limitations.md @@ -1,4 +1,4 @@ -# Design limitation +# Limitations The document describes the limitations that are currently present in the `behavior_path_planner` module. diff --git a/planning/behavior_path_planner/behavior_path_planner_path_generation.md b/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md similarity index 94% rename from planning/behavior_path_planner/behavior_path_planner_path_generation.md rename to planning/behavior_path_planner/behavior_path_planner_path_generation_design.md index da2ea34f1d673..550d37a4a4ea1 100644 --- a/planning/behavior_path_planner/behavior_path_planner_path_generation.md +++ b/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md @@ -1,8 +1,8 @@ -## Path Generation +# Path Generation design This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp). -### Overview +## Overview The base idea of the path generation in lane change and avoidance is to smoothly shift the reference path, such as the center line, in the lateral direction. This is achieved by using a constant-jerk profile as in the figure below. More details on how it is used can be found in [README](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/README.md). It is assumed that the reference path is smooth enough for this algorithm. @@ -14,7 +14,7 @@ The figure below explains how the application of a constant lateral jerk $l^{''' Note that, due to the rarity of the $T_v$ in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation. -### Mathematical Derivation +## Mathematical Derivation By applying simple integral operations, the following analytical equations can be derived to describe the shift distance $l(t)$ at each time under lateral jerk, acceleration, and velocity constraints. @@ -32,7 +32,7 @@ l_7&= 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j + j(T_a + T_j)T_j T_v These equations are used to determine the shape of a path. Additionally, by applying further mathematical operations to these basic equations, the expressions of the following subsections can be derived. -#### Calculation of Maximum Acceleration from transition time and final shift length +### Calculation of Maximum Acceleration from transition time and final shift length In the case where there are no limitations on lateral velocity and lateral acceleration, the maximum lateral acceleration during the shifting can be calculated as follows. The constant-jerk time is given by $T_j = T_{\rm total}/4$ because of its symmetric property. Since $T_a=T_v=0$, the final shift length $L=l_7=2jT_j^3$ can be determined using the above equation. The maximum lateral acceleration is then given by $a_{\rm max} =jT_j$. This results in the following expression for the maximum lateral acceleration: @@ -42,7 +42,7 @@ a_{\rm max} = \frac{8L}{T_{\rm total}^2} \end{align} ``` -#### Calculation of Ta, Tj and jerk from acceleration limit +### Calculation of Ta, Tj and jerk from acceleration limit In the case where there are no limitations on lateral velocity, the constant-jerk and acceleration times, as well as the required jerk can be calculated from the acceleration limit, total time, and final shift length as follows. Since $T_v=0$, the final shift length is given by: @@ -73,7 +73,7 @@ jerk&=\frac{2a_{\rm lim} ^2T_{\rm total}}{a_{\rm lim} T_{\rm total}^2-4L} where $T_j$ is the constant-jerk time, $T_a$ is the constant acceleration time, $j$ is the required jerk, $a_{\rm lim}$ is the acceleration limit, and $L$ is the final shift length. -#### Calculation of Required Time from Jerk and Acceleration Constraint +### Calculation of Required Time from Jerk and Acceleration Constraint In the case where there are no limitations on lateral velocity, the total time required for shifting can be calculated from the jerk and acceleration limits and the final shift length as follows. By solving the two equations given above: @@ -92,6 +92,6 @@ T_a &= \frac{1}{2}\sqrt{\frac{a_{\rm lim}}{j}^2 + \frac{4L}{a_{\rm lim}}} - \fra The total time required for shifting can then be calculated as $T_{\rm total}=4T_j+2T_a$. -### Limitation +## Limitation - Since $T_v$ is zero in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation. diff --git a/planning/behavior_path_planner/behavior_path_planner_side-shift.md b/planning/behavior_path_planner/behavior_path_planner_side_shift_design.md similarity index 99% rename from planning/behavior_path_planner/behavior_path_planner_side-shift.md rename to planning/behavior_path_planner/behavior_path_planner_side_shift_design.md index 22c55ddc7370b..b76e549eb602f 100644 --- a/planning/behavior_path_planner/behavior_path_planner_side-shift.md +++ b/planning/behavior_path_planner/behavior_path_planner_side_shift_design.md @@ -1,4 +1,4 @@ -# Side shift Module +# Side Shift design (For remote control) Shift the path to left or right according to an external instruction. diff --git a/planning/behavior_path_planner/behavior_path_planner_turn_signal-design.md b/planning/behavior_path_planner/behavior_path_planner_turn_signal_design.md similarity index 99% rename from planning/behavior_path_planner/behavior_path_planner_turn_signal-design.md rename to planning/behavior_path_planner/behavior_path_planner_turn_signal_design.md index 8218337313e5c..8ee9a8f40a346 100644 --- a/planning/behavior_path_planner/behavior_path_planner_turn_signal-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_turn_signal_design.md @@ -1,4 +1,4 @@ -# Turn Signal Decider +# Turn Signal design Turn Signal decider determines necessary blinkers. From 102d974a67ebda7dc0da34bf8fc2d90218128bac Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 28 Feb 2023 17:22:12 +0900 Subject: [PATCH 03/24] feat(motion_velocity_smoother): improve velocity planning around stop point (#2955) * feat(motion_velocity_smoother): remove unnecessary constant value Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../src/smoother/jerk_filtered_smoother.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 62d680af2d72b..580d8e0c44f5a 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -187,10 +187,9 @@ bool JerkFilteredSmoother::apply( /**************************************************************/ // jerk: d(ai)/ds * v_ref -> minimize weight * ((a1 - a0) / ds * v_ref)^2 * ds - constexpr double ZERO_VEL_THR_FOR_DT_CALC = 0.3; const double smooth_weight = smoother_param_.jerk_weight; for (size_t i = 0; i < N - 1; ++i) { - const double ref_vel = v_max_arr.at(i); + const double ref_vel = 0.5 * (v_max_arr.at(i) + v_max_arr.at(i + 1)); const double interval_dist = std::max(interval_dist_arr.at(i), 0.0001); const double w_x_ds_inv = (1.0 / interval_dist) * ref_vel; P(IDX_A0 + i, IDX_A0 + i) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; @@ -254,7 +253,7 @@ bool JerkFilteredSmoother::apply( // Soft Constraint Jerk Limit: jerk_min < pseudo_jerk[i] * ref_vel[i] - gamma[i] < jerk_max // -> jerk_min * ds < (a[i+1] - a[i]) * ref_vel[i] - gamma[i] * ds < jerk_max * ds for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { - const double ref_vel = std::max(v_max_arr.at(i), ZERO_VEL_THR_FOR_DT_CALC); + const double ref_vel = 0.5 * (v_max_arr.at(i) + v_max_arr.at(i + 1)); const double ds = interval_dist_arr.at(i); A(constr_idx, IDX_A0 + i) = -ref_vel; // -a[i] * ref_vel A(constr_idx, IDX_A0 + i + 1) = ref_vel; // a[i+1] * ref_vel From f05708429824bed47a6d1f18212934a7611eff36 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 1 Mar 2023 07:07:28 +0900 Subject: [PATCH 04/24] feat(behavior_path_planner): add new manager independent from behavior tree (#2919) * feat(data_manager): add member variable for new framework Signed-off-by: satoshi-ota * feat(utilities): add util function Signed-off-by: satoshi-ota * feat(behavior_path_planner): add framework that based on new architecture Signed-off-by: satoshi-ota * fix(behavior_path_planner): changeable framework by macro Signed-off-by: satoshi-ota * feat(behavior_path_planner): change framework in cmake Signed-off-by: satoshi-ota * fix(behavior_path_planner): fix cmake lint error Signed-off-by: satoshi-ota * fix(planner_manager): rename functions Signed-off-by: satoshi-ota * fix(planner_manager): not use ModuleID Signed-off-by: satoshi-ota * refactor(planner_manager): use lambda Signed-off-by: satoshi-ota * feat(planner_manager): reserve vector Signed-off-by: satoshi-ota * fix(behavior_path_planner): fix copyright Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * fix(behavior_path_planner): fix copyright Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * fix(behavior_path_planner): fix copyright Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --------- Signed-off-by: satoshi-ota Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- planning/behavior_path_planner/CMakeLists.txt | 55 ++- .../behavior_path_planner_node.hpp | 40 +- .../behavior_path_planner/data_manager.hpp | 6 + .../behavior_path_planner/parameters.hpp | 2 + .../behavior_path_planner/planner_manager.hpp | 206 ++++++++ .../scene_module_interface.hpp | 298 ++++++++++++ .../scene_module_manager_interface.hpp | 190 ++++++++ .../behavior_path_planner/utilities.hpp | 3 + .../src/behavior_path_planner_node.cpp | 179 ++++++- .../src/planner_manager.cpp | 452 ++++++++++++++++++ .../src/scene_module/scene_module_visitor.cpp | 4 + .../behavior_path_planner/src/utilities.cpp | 23 + 12 files changed, 1427 insertions(+), 31 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp create mode 100644 planning/behavior_path_planner/src/planner_manager.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index a30254b4a8ecd..f290e8b8a3dab 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,22 +7,15 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -ament_auto_add_library(behavior_path_planner_node SHARED - src/behavior_path_planner_node.cpp - src/behavior_tree_manager.cpp +# use planner manager that supports behavior tree +set(USE_BT TRUE) +message(STATUS "USE_BEHAVIOR_TREE: " ${USE_BT}) + +set(common_src src/utilities.cpp src/path_utilities.cpp src/steering_factor_interface.cpp src/turn_signal_decider.cpp - src/scene_module/scene_module_bt_node_interface.cpp - src/scene_module/side_shift/side_shift_module.cpp - src/scene_module/avoidance/avoidance_module.cpp - src/scene_module/lane_following/lane_following_module.cpp - src/scene_module/lane_change/external_request_lane_change_module.cpp - src/scene_module/lane_change/lane_change_module.cpp - src/scene_module/pull_over/pull_over_module.cpp - src/scene_module/pull_out/pull_out_module.cpp - src/scene_module/scene_module_visitor.cpp src/util/avoidance/util.cpp src/util/lane_change/util.cpp src/util/side_shift/util.cpp @@ -33,18 +26,52 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/util/pull_out/util.cpp src/util/pull_out/shift_pull_out.cpp src/util/pull_out/geometric_pull_out.cpp - src/util/geometric_parallel_parking/geometric_parallel_parking.cpp - src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/util/path_shifter/path_shifter.cpp src/util/drivable_area_expansion/drivable_area_expansion.cpp src/util/drivable_area_expansion/map_utils.cpp src/util/drivable_area_expansion/footprints.cpp src/util/drivable_area_expansion/expansion.cpp + src/util/geometric_parallel_parking/geometric_parallel_parking.cpp + src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_util/debug_utilities.cpp src/marker_util/avoidance/debug.cpp src/marker_util/lane_change/debug.cpp ) +if(USE_BT) + ament_auto_add_library(behavior_path_planner_node SHARED + src/behavior_path_planner_node.cpp + src/behavior_tree_manager.cpp + src/scene_module/scene_module_visitor.cpp + src/scene_module/scene_module_bt_node_interface.cpp + src/scene_module/side_shift/side_shift_module.cpp + src/scene_module/avoidance/avoidance_module.cpp + src/scene_module/lane_following/lane_following_module.cpp + src/scene_module/lane_change/external_request_lane_change_module.cpp + src/scene_module/lane_change/lane_change_module.cpp + src/scene_module/pull_over/pull_over_module.cpp + src/scene_module/pull_out/pull_out_module.cpp + ${common_src} + ) + + target_compile_definitions(behavior_path_planner_node PRIVATE USE_BEHAVIOR_TREE) + + message(WARNING "Build behavior_path_planner with BT...") + +else() + ament_auto_add_library(behavior_path_planner_node SHARED + src/behavior_path_planner_node.cpp + src/planner_manager.cpp + src/scene_module/scene_module_visitor.cpp + # src/scene_module_v2/avoidance + # src/scene_module_v2/lane_change + # src/scene_module_v2/... + ${common_src} + ) + + message(WARNING "Build behavior_path_planner without BT...") +endif() + target_include_directories(behavior_path_planner_node SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 95e8397b4ed33..de3858560be98 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -15,19 +15,28 @@ #ifndef BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ #define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ -#include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/data_manager.hpp" + +#ifdef USE_BEHAVIOR_TREE +#include "behavior_path_planner/behavior_tree_manager.hpp" +#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" +#else +#include "behavior_path_planner/planner_manager.hpp" +#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" +#endif + #include "behavior_path_planner/steering_factor_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" +#include #include #include #include @@ -65,6 +74,7 @@ inline void update_param( namespace behavior_path_planner { +using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; @@ -97,6 +107,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; +#ifndef USE_BEHAVIOR_TREE + rclcpp::Subscription::SharedPtr operation_mode_subscriber_; +#endif rclcpp::Publisher::SharedPtr path_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; @@ -105,9 +118,18 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; +#ifndef USE_BEHAVIOR_TREE + std::map::SharedPtr> path_reference_publishers_; +#endif std::shared_ptr planner_data_; + +#ifdef USE_BEHAVIOR_TREE std::shared_ptr bt_manager_; +#else + std::shared_ptr planner_manager_; +#endif + std::unique_ptr steering_factor_interface_ptr_; Scenario::SharedPtr current_scenario_{nullptr}; @@ -127,11 +149,15 @@ class BehaviorPathPlannerNode : public rclcpp::Node // update planner data std::shared_ptr createLatestPlannerData(); +#ifdef USE_BEHAVIOR_TREE // parameters std::shared_ptr avoidance_param_ptr; std::shared_ptr lane_change_param_ptr; +#endif BehaviorPathPlannerParameters getCommonParam(); + +#ifdef USE_BEHAVIOR_TREE BehaviorTreeManagerParam getBehaviorTreeManagerParam(); SideShiftParameters getSideShiftParam(); AvoidanceParameters getAvoidanceParam(); @@ -139,6 +165,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node LaneChangeParameters getLaneChangeParam(); PullOverParameters getPullOverParam(); PullOutParameters getPullOutParam(); +#endif // callback void onOdometry(const Odometry::ConstSharedPtr msg); @@ -147,6 +174,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); +#ifndef USE_BEHAVIOR_TREE + void onOperationMode(const OperationModeState::ConstSharedPtr msg); +#endif SetParametersResult onSetParam(const std::vector & parameters); /** @@ -205,8 +235,16 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish path candidate */ +#ifdef USE_BEHAVIOR_TREE void publishPathCandidate( const std::vector> & scene_modules); +#else + void publishPathCandidate( + const std::vector> & managers); + + void publishPathReference( + const std::vector> & managers); +#endif /** * @brief convert path with lane id to path for publish path candidate diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 0cbe57bc384a2..a644fa955ed23 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -40,6 +41,7 @@ namespace behavior_path_planner { +using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; @@ -97,6 +99,9 @@ struct BehaviorModuleOutput // path planed by module PlanResult path{}; + // reference path planed by module + PlanResult reference_path{}; + TurnSignalInfo turn_signal_info{}; std::optional modified_goal{}; @@ -118,6 +123,7 @@ struct PlannerData AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{}; PredictedObjects::ConstSharedPtr dynamic_object{}; OccupancyGrid::ConstSharedPtr occupancy_grid{}; + OperationModeState::ConstSharedPtr operation_mode{}; PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; lanelet::ConstLanelets current_lanes{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 7caa25a9b4cf6..ea38ed2cb61cf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -19,6 +19,8 @@ struct BehaviorPathPlannerParameters { + bool verbose; + double backward_path_length; double forward_path_length; double backward_length_buffer_for_end_of_lane; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp new file mode 100644 index 0000000000000..517c9fd355048 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -0,0 +1,206 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ + +#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" +#include "behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ + +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_autoware_utils::StopWatch; +using SceneModulePtr = std::shared_ptr; +using SceneModuleManagerPtr = std::shared_ptr; + +struct SceneModuleStatus +{ + explicit SceneModuleStatus(const std::string & n) : module_name(n) {} + + std::string module_name; + + bool is_execution_ready{false}; + bool is_waiting_approval{false}; + + ModuleStatus status{ModuleStatus::SUCCESS}; +}; + +class PlannerManager +{ +public: + PlannerManager(rclcpp::Node & node, const bool verbose); + + BehaviorModuleOutput run(const std::shared_ptr & data); + + void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr) + { + RCLCPP_INFO(logger_, "register %s module", manager_ptr->getModuleName().c_str()); + manager_ptrs_.push_back(manager_ptr); + processing_time_.emplace(manager_ptr->getModuleName(), 0.0); + } + + void updateModuleParams(const std::vector & parameters) + { + std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [¶meters](const auto & m) { + m->updateModuleParams(parameters); + }); + } + + void reset() + { + approved_module_ptrs_.clear(); + candidate_module_opt_ = boost::none; + root_lanelet_ = boost::none; + std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); + resetProcessingTime(); + } + + void publishDebugMarker() const + { + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishDebugMarker(); }); + } + + std::vector getSceneModuleManagers() const { return manager_ptrs_; } + + std::vector> getSceneModuleStatus() const + { + std::vector> ret; + + const auto size = approved_module_ptrs_.size() + 1; + + ret.reserve(size); + + for (const auto & m : approved_module_ptrs_) { + auto s = std::make_shared(m->name()); + s->is_waiting_approval = m->isWaitingApproval(); + s->status = m->getCurrentStatus(); + ret.push_back(s); + } + + if (!!candidate_module_opt_) { + const auto m = candidate_module_opt_.get(); + auto s = std::make_shared(m->name()); + s->is_waiting_approval = m->isWaitingApproval(); + s->status = m->getCurrentStatus(); + ret.push_back(s); + } + + ret.shrink_to_fit(); + + return ret; + } + + void resetRootLanelet(const std::shared_ptr & data); + + void print() const; + +private: + BehaviorModuleOutput run( + const SceneModulePtr & module_ptr, const std::shared_ptr & planner_data, + const BehaviorModuleOutput & previous_module_output) const + { + module_ptr->setData(planner_data); + module_ptr->setPreviousModuleOutput(previous_module_output); + module_ptr->updateData(); + + module_ptr->lockRTCCommand(); + const auto result = module_ptr->run(); + module_ptr->unlockRTCCommand(); + + module_ptr->updateState(); + + module_ptr->publishRTCStatus(); + + return result; + } + + void deleteExpiredModules(const SceneModulePtr & module_ptr) const + { + const auto itr = std::find_if( + manager_ptrs_.begin(), manager_ptrs_.end(), + [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); }); + if (itr == manager_ptrs_.end()) { + return; + } + + (*itr)->deleteModules(module_ptr); + } + + void addApprovedModule(const SceneModulePtr & module_ptr) + { + approved_module_ptrs_.push_back(module_ptr); + } + + void resetProcessingTime() + { + for (auto & t : processing_time_) { + t.second = 0.0; + } + } + + lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const + { + lanelet::ConstLanelet ret{}; + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); + return ret; + } + + BehaviorModuleOutput update(const std::shared_ptr & data); + + BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const; + + boost::optional getCandidateModule( + const BehaviorModuleOutput & previous_module_output) const; + + boost::optional> selectHighestPriorityModule( + std::vector> & request_modules) const; + + boost::optional candidate_module_opt_{boost::none}; + + boost::optional root_lanelet_{boost::none}; + + std::vector manager_ptrs_; + + std::vector approved_module_ptrs_; + + rclcpp::Logger logger_; + + rclcpp::Clock clock_; + + mutable StopWatch stop_watch_; + + mutable std::unordered_map processing_time_; + + bool verbose_{false}; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.hpp new file mode 100644 index 0000000000000..3a5b04ed0af97 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_INTERFACE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_INTERFACE_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#include "behavior_path_planner/utilities.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using rtc_interface::RTCInterface; +using steering_factor_interface::SteeringFactorInterface; +using tier4_autoware_utils::generateUUID; +using unique_identifier_msgs::msg::UUID; +using visualization_msgs::msg::MarkerArray; +using PlanResult = PathWithLaneId::SharedPtr; + +enum class ModuleStatus { + IDLE = 0, + RUNNING = 1, + SUCCESS = 2, + FAILURE = 3, + // SKIPPED = 4, +}; + +class SceneModuleInterface +{ +public: + SceneModuleInterface(const std::string & name, rclcpp::Node & node) + : name_{name}, + logger_{node.get_logger().get_child(name)}, + clock_{node.get_clock()}, + is_waiting_approval_{false}, + is_locked_new_module_launch_{false}, + uuid_(generateUUID()), + current_state_{ModuleStatus::SUCCESS} + { + } + + virtual ~SceneModuleInterface() = default; + + /** + * @brief Return SUCCESS if plan is not needed or plan is successfully finished, + * FAILURE if plan has failed, RUNNING if plan is on going. + * These condition is to be implemented in each modules. + */ + virtual ModuleStatus updateState() = 0; + + /** + * @brief If the module plan customized reference path while waiting approval, it should output + * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. + */ + virtual ModuleStatus getNodeStatusWhileWaitingApproval() const { return ModuleStatus::FAILURE; } + + /** + * @brief Return true if the module has request for execution (not necessarily feasible) + */ + virtual bool isExecutionRequested() const = 0; + + /** + * @brief Return true if the execution is available (e.g. safety check for lane change) + */ + virtual bool isExecutionReady() const = 0; + + /** + * @brief Calculate path. This function is called with the plan is approved. + */ + virtual BehaviorModuleOutput plan() = 0; + + /** + * @brief Calculate path under waiting_approval condition. + * The default implementation is just to return the reference path. + */ + virtual BehaviorModuleOutput planWaitingApproval() + { + BehaviorModuleOutput out; + out.path = util::generateCenterLinePath(planner_data_); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + path_reference_ = previous_module_output_.reference_path; + return out; + } + + /** + * @brief Get candidate path. This information is used for external judgement. + */ + virtual CandidateOutput planCandidate() const = 0; + + /** + * @brief update data for planning. Note that the call of this function does not mean + * that the module executed. It should only updates the data necessary for + * planCandidate (e.g., resampling of path). + */ + virtual void updateData() {} + + /** + * @brief Execute module. Once this function is executed, + * it will continue to run as long as it is in the RUNNING state. + */ + virtual BehaviorModuleOutput run() + { + current_state_ = ModuleStatus::RUNNING; + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(logger_, "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(logger_, "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } + } + + /** + * @brief Called on the first time when the module goes into RUNNING. + */ + virtual void onEntry() = 0; + + /** + * @brief Called when the module exit from RUNNING. + */ + virtual void onExit() = 0; + + /** + * @brief Publish status if the module is requested to run + */ + virtual void publishRTCStatus() + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->publishCooperateStatus(clock_->now()); + } + + /** + * @brief Return true if the activation command is received + */ + virtual bool isActivated() + { + if (!rtc_interface_ptr_) { + return true; + } + if (rtc_interface_ptr_->isRegistered(uuid_)) { + return rtc_interface_ptr_->isActivated(uuid_); + } + return false; + } + + virtual void publishSteeringFactor() + { + if (!steering_factor_interface_ptr_) { + return; + } + steering_factor_interface_ptr_->publishSteeringFactor(clock_->now()); + } + + virtual void lockRTCCommand() + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->lockCommandUpdate(); + } + + virtual void unlockRTCCommand() + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->unlockCommandUpdate(); + } + + /** + * @brief set previous module's output as input for this module + */ + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + + /** + * @brief set planner data + */ + void setData(const std::shared_ptr & data) { planner_data_ = data; } + + void resetPathCandidate() { path_candidate_.reset(); } + + void resetPathReference() { path_reference_.reset(); } + + bool isWaitingApproval() const { return is_waiting_approval_; } + + bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } + + rclcpp::Logger getLogger() const { return logger_; } + + PlanResult getPathCandidate() const { return path_candidate_; } + + PlanResult getPathReference() const { return path_reference_; } + + MarkerArray getDebugMarkers() { return debug_marker_; } + + ModuleStatus getCurrentStatus() const { return current_state_; } + + virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; + + std::string name() const { return name_; } + + std::shared_ptr planner_data_; + +private: + std::string name_; + rclcpp::Logger logger_; + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->updateCooperateStatus( + uuid_, isExecutionReady(), start_distance, finish_distance, clock_->now()); + } + + virtual void removeRTCStatus() + { + if (!rtc_interface_ptr_) { + return; + } + rtc_interface_ptr_->clearCooperateStatus(); + } + + void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } + + void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; } + + void waitApproval() { is_waiting_approval_ = true; } + + void clearWaitingApproval() { is_waiting_approval_ = false; } + + rclcpp::Clock::SharedPtr clock_; + + std::shared_ptr rtc_interface_ptr_; + std::unique_ptr steering_factor_interface_ptr_; + + bool is_waiting_approval_; + bool is_locked_new_module_launch_; + + UUID uuid_; + + PlanResult path_candidate_; + PlanResult path_reference_; + + ModuleStatus current_state_; + + BehaviorModuleOutput previous_module_output_; + + mutable MarkerArray debug_marker_; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp new file mode 100644 index 0000000000000..cdc0e0b0a3405 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp @@ -0,0 +1,190 @@ + +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_MANAGER_INTERFACE_HPP_ + +#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ + +using tier4_autoware_utils::toHexString; +using unique_identifier_msgs::msg::UUID; +using SceneModulePtr = std::shared_ptr; + +class SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterface( + rclcpp::Node * node, const std::string & name, const size_t max_module_num, + const size_t priority, const bool enable_simultaneous_execution) + : node_(node), + clock_(*node->get_clock()), + logger_(node->get_logger().get_child(name)), + name_(name), + max_module_num_(max_module_num), + priority_(priority), + enable_simultaneous_execution_(enable_simultaneous_execution) + { + pub_debug_marker_ = node->create_publisher("~/debug/" + name, 20); + } + + virtual ~SceneModuleManagerInterface() = default; + + SceneModulePtr getNewModule() + { + if (idling_module_ != nullptr) { + return idling_module_; + } + + idling_module_ = createNewSceneModuleInstance(); + return idling_module_; + } + + bool isExecutionRequested( + const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) const + { + module_ptr->setData(planner_data_); + module_ptr->setPreviousModuleOutput(previous_module_output); + module_ptr->updateData(); + + return module_ptr->isExecutionRequested(); + } + + void registerNewModule( + const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) + { + module_ptr->setData(planner_data_); + module_ptr->setPreviousModuleOutput(previous_module_output); + module_ptr->onEntry(); + + registered_modules_.push_back(module_ptr); + } + + void deleteModules(const SceneModulePtr & module_ptr) + { + module_ptr->onExit(); + module_ptr->publishRTCStatus(); + + const auto itr = std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr); + + registered_modules_.erase(itr); + + pub_debug_marker_->publish(MarkerArray{}); + } + + void publishDebugMarker() const + { + using tier4_autoware_utils::appendMarkerArray; + + MarkerArray markers{}; + + const auto marker_offset = std::numeric_limits::max(); + + uint32_t marker_id = marker_offset; + for (const auto & m : registered_modules_) { + for (auto & marker : m->getDebugMarkers().markers) { + marker.id += marker_id; + markers.markers.push_back(marker); + } + marker_id += marker_offset; + } + + if (registered_modules_.empty() && idling_module_ != nullptr) { + appendMarkerArray(idling_module_->getDebugMarkers(), &markers); + } + + pub_debug_marker_->publish(markers); + } + + bool exist(const SceneModulePtr & module_ptr) const + { + return std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr) != + registered_modules_.end(); + } + + bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; } + + bool isSimultaneousExecutable() const { return enable_simultaneous_execution_; } + + void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } + + void reset() + { + std::for_each(registered_modules_.begin(), registered_modules_.end(), [](const auto & m) { + m->onExit(); + m->publishRTCStatus(); + }); + registered_modules_.clear(); + + idling_module_->onExit(); + idling_module_->publishRTCStatus(); + idling_module_.reset(); + + pub_debug_marker_->publish(MarkerArray{}); + } + + size_t getPriority() const { return priority_; } + + std::string getModuleName() const { return name_; } + + std::vector getSceneModules() { return registered_modules_; } + + virtual void updateModuleParams(const std::vector & parameters) = 0; + +protected: + virtual void getModuleParams(rclcpp::Node * node) = 0; + + virtual std::shared_ptr createNewSceneModuleInstance() = 0; + + rclcpp::Node * node_; + + rclcpp::Clock clock_; + + rclcpp::Logger logger_; + + rclcpp::Publisher::SharedPtr pub_debug_marker_; + + std::string name_; + + std::shared_ptr planner_data_; + + std::vector registered_modules_; + + SceneModulePtr idling_module_; + +private: + size_t max_module_num_; + + size_t priority_; + + bool enable_simultaneous_execution_{false}; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_MANAGER_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 4b22fce427764..91058b2cccdb3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -517,6 +517,9 @@ double calcTotalLaneChangeDistance( double calcLaneChangeBuffer( const BehaviorPathPlannerParameters & common_param, const int num_lane_change, const double length_to_intersection = 0.0); + +lanelet::ConstLanelets getLaneletsFromPath( + const PathWithLaneId & path, const std::shared_ptr & route_handler); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 032f43aead3f3..601542fd34d43 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -16,7 +16,6 @@ #include "behavior_path_planner/marker_util/debug_utilities.hpp" #include "behavior_path_planner/path_utilities.hpp" -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" #include "behavior_path_planner/util/drivable_area_expansion/map_utils.hpp" #include @@ -92,6 +91,12 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod occupancy_grid_subscriber_ = create_subscription( "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); +#ifndef USE_BEHAVIOR_TREE + operation_mode_subscriber_ = create_subscription( + "/system/operation_mode/state", 1, + std::bind(&BehaviorPathPlannerNode::onOperationMode, this, _1), + createSubscriptionOptions(this)); +#endif scenario_subscriber_ = create_subscription( "~/input/scenario", 1, [this](const Scenario::ConstSharedPtr msg) { @@ -108,13 +113,19 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1), createSubscriptionOptions(this)); +#ifdef USE_BEHAVIOR_TREE avoidance_param_ptr = std::make_shared(getAvoidanceParam()); lane_change_param_ptr = std::make_shared(getLaneChangeParam()); +#endif m_set_param_res = this->add_on_set_parameters_callback( std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); + +#ifdef USE_BEHAVIOR_TREE // behavior tree manager { + RCLCPP_INFO(get_logger(), "use behavior tree."); + const std::string path_candidate_name_space = "/planning/path_candidate/"; mutex_bt_.lock(); @@ -170,6 +181,21 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod mutex_bt_.unlock(); } +#else + { + RCLCPP_INFO(get_logger(), "not use behavior tree."); + + const std::string path_candidate_name_space = "/planning/path_candidate/"; + const std::string path_reference_name_space = "/planning/path_reference/"; + + mutex_bt_.lock(); + + const auto & p = planner_data_->parameters; + planner_manager_ = std::make_shared(*this, p.verbose); + + mutex_bt_.unlock(); + } +#endif // turn signal decider { @@ -198,6 +224,10 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; +#ifndef USE_BEHAVIOR_TREE + p.verbose = declare_parameter("verbose"); +#endif + // vehicle info const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); p.vehicle_info = vehicle_info; @@ -275,6 +305,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() return p; } +#ifdef USE_BEHAVIOR_TREE SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -296,7 +327,9 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() return p; } +#endif +#ifdef USE_BEHAVIOR_TREE AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() { AvoidanceParameters p{}; @@ -450,7 +483,9 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() return p; } +#endif +#ifdef USE_BEHAVIOR_TREE LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() { LaneFollowingParameters p{}; @@ -464,7 +499,9 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() declare_parameter("lane_following.lane_change_prepare_duration", 2.0); return p; } +#endif +#ifdef USE_BEHAVIOR_TREE LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -541,7 +578,9 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() return p; } +#endif +#ifdef USE_BEHAVIOR_TREE PullOverParameters BehaviorPathPlannerNode::getPullOverParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -637,7 +676,9 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() return p; } +#endif +#ifdef USE_BEHAVIOR_TREE PullOutParameters BehaviorPathPlannerNode::getPullOutParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -693,7 +734,9 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() return p; } +#endif +#ifdef USE_BEHAVIOR_TREE BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() { BehaviorTreeManagerParam p{}; @@ -702,6 +745,7 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() p.groot_zmq_server_port = declare_parameter("groot_zmq_server_port", 1667); return p; } +#endif // wait until mandatory data is ready bool BehaviorPathPlannerNode::isDataReady() @@ -735,6 +779,12 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("self_acceleration"); } +#ifndef USE_BEHAVIOR_TREE + if (!planner_data_->operation_mode) { + return missing("operation_mode"); + } +#endif + return true; } @@ -756,7 +806,11 @@ std::shared_ptr BehaviorPathPlannerNode::createLatestPlannerData() // so that the each modules do not have to care about the "route jump". if (!is_first_time) { RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); +#ifdef USE_BEHAVIOR_TREE bt_manager_->resetBehaviorTree(); +#else + planner_manager_->reset(); +#endif } has_received_route_ = false; @@ -783,8 +837,18 @@ void BehaviorPathPlannerNode::run() // create latest planner data const auto planner_data = createLatestPlannerData(); +#ifndef USE_BEHAVIOR_TREE + if (planner_data->operation_mode->mode != OperationModeState::AUTONOMOUS) { + planner_manager_->resetRootLanelet(planner_data); + } +#endif + // run behavior planner +#ifdef USE_BEHAVIOR_TREE const auto output = bt_manager_->run(planner_data); +#else + const auto output = planner_manager_->run(planner_data); +#endif // path handling const auto path = getPath(output, planner_data); @@ -808,7 +872,12 @@ void BehaviorPathPlannerNode::run() get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); } +#ifdef USE_BEHAVIOR_TREE publishPathCandidate(bt_manager_->getSceneModules()); +#else + publishPathCandidate(planner_manager_->getSceneModuleManagers()); + publishPathReference(planner_manager_->getSceneModuleManagers()); +#endif publishSceneModuleDebugMsg(); @@ -824,6 +893,11 @@ void BehaviorPathPlannerNode::run() debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); } +#ifndef USE_BEHAVIOR_TREE + planner_manager_->print(); + planner_manager_->publishDebugMarker(); +#endif + mutex_bt_.unlock(); RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } @@ -914,20 +988,22 @@ void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) void BehaviorPathPlannerNode::publishSceneModuleDebugMsg() { - { - const auto debug_messages_data_ptr = bt_manager_->getAllSceneModuleDebugMsgData(); - const auto avoidance_debug_message = debug_messages_data_ptr->getAvoidanceModuleDebugMsg(); - if (avoidance_debug_message) { - debug_avoidance_msg_array_publisher_->publish(*avoidance_debug_message); - } +#ifdef USE_BEHAVIOR_TREE + const auto debug_messages_data_ptr = bt_manager_->getAllSceneModuleDebugMsgData(); - const auto lane_change_debug_message = debug_messages_data_ptr->getLaneChangeModuleDebugMsg(); - if (lane_change_debug_message) { - debug_lane_change_msg_array_publisher_->publish(*lane_change_debug_message); - } + const auto avoidance_debug_message = debug_messages_data_ptr->getAvoidanceModuleDebugMsg(); + if (avoidance_debug_message) { + debug_avoidance_msg_array_publisher_->publish(*avoidance_debug_message); } + + const auto lane_change_debug_message = debug_messages_data_ptr->getLaneChangeModuleDebugMsg(); + if (lane_change_debug_message) { + debug_lane_change_msg_array_publisher_->publish(*lane_change_debug_message); + } +#endif } +#ifdef USE_BEHAVIOR_TREE void BehaviorPathPlannerNode::publishPathCandidate( const std::vector> & scene_modules) { @@ -938,6 +1014,49 @@ void BehaviorPathPlannerNode::publishPathCandidate( } } } +#else +void BehaviorPathPlannerNode::publishPathCandidate( + const std::vector> & managers) +{ + for (auto & manager : managers) { + if (path_candidate_publishers_.count(manager->getModuleName()) == 0) { + continue; + } + + if (manager->getSceneModules().empty()) { + path_candidate_publishers_.at(manager->getModuleName()) + ->publish(convertToPath(nullptr, false)); + continue; + } + + for (auto & module : manager->getSceneModules()) { + path_candidate_publishers_.at(module->name()) + ->publish(convertToPath(module->getPathCandidate(), module->isExecutionReady())); + } + } +} + +void BehaviorPathPlannerNode::publishPathReference( + const std::vector> & managers) +{ + for (auto & manager : managers) { + if (path_reference_publishers_.count(manager->getModuleName()) == 0) { + continue; + } + + if (manager->getSceneModules().empty()) { + path_reference_publishers_.at(manager->getModuleName()) + ->publish(convertToPath(nullptr, false)); + continue; + } + + for (auto & module : manager->getSceneModules()) { + path_reference_publishers_.at(module->name()) + ->publish(convertToPath(module->getPathReference(), true)); + } + } +} +#endif Path BehaviorPathPlannerNode::convertToPath( const std::shared_ptr & path_candidate_ptr, const bool is_ready) @@ -976,7 +1095,11 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); PathWithLaneId connected_path; +#ifdef USE_BEHAVIOR_TREE const auto module_status_ptr_vec = bt_manager_->getModulesStatus(); +#else + const auto module_status_ptr_vec = planner_manager_->getSceneModuleStatus(); +#endif if (skipSmoothGoalConnection(module_status_ptr_vec)) { connected_path = *path; } else { @@ -994,8 +1117,14 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( { const auto target_module = "PullOver"; +#ifdef USE_BEHAVIOR_TREE + const auto target_status = BT::NodeStatus::RUNNING; +#else + const auto target_status = ModuleStatus::RUNNING; +#endif + for (auto & status : statuses) { - if (status->is_waiting_approval || status->status == BT::NodeStatus::RUNNING) { + if (status->is_waiting_approval || status->status == target_status) { if (target_module == status->module_name) { return true; } @@ -1008,12 +1137,19 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( bool BehaviorPathPlannerNode::keepInputPoints( const std::vector> & statuses) const { - const auto target_module_1 = "PullOver"; - const auto target_module_2 = "Avoidance"; + const std::vector target_modules = {"PullOver", "Avoidance"}; + +#ifdef USE_BEHAVIOR_TREE + const auto target_status = BT::NodeStatus::RUNNING; +#else + const auto target_status = ModuleStatus::RUNNING; +#endif for (auto & status : statuses) { - if (status->is_waiting_approval || status->status == BT::NodeStatus::RUNNING) { - if (target_module_1 == status->module_name || target_module_2 == status->module_name) { + if (status->is_waiting_approval || status->status == target_status) { + if ( + std::find(target_modules.begin(), target_modules.end(), status->module_name) != + target_modules.end()) { return true; } } @@ -1053,26 +1189,37 @@ void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) route_ptr_ = msg; has_received_route_ = true; } +#ifndef USE_BEHAVIOR_TREE +void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + const std::lock_guard lock(mutex_pd_); + planner_data_->operation_mode = msg; +} +#endif SetParametersResult BehaviorPathPlannerNode::onSetParam( const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; +#ifdef USE_BEHAVIOR_TREE if (!lane_change_param_ptr && !avoidance_param_ptr) { result.successful = false; result.reason = "param not initialized"; return result; } +#endif result.successful = true; result.reason = "success"; try { +#ifdef USE_BEHAVIOR_TREE update_param( parameters, "avoidance.publish_debug_marker", avoidance_param_ptr->publish_debug_marker); update_param( parameters, "lane_change.publish_debug_marker", lane_change_param_ptr->publish_debug_marker); +#endif // Drivable area expansion parameters using drivable_area_expansion::DrivableAreaExpansionParameters; update_param( diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp new file mode 100644 index 0000000000000..32b246c618f18 --- /dev/null +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -0,0 +1,452 @@ +// Copyright 2023 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 "behavior_path_planner/planner_manager.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/utilities.hpp" + +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ + +PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) +: logger_(node.get_logger().get_child("planner_manager")), + clock_(*node.get_clock()), + verbose_{verbose} +{ + processing_time_.emplace("total_time", 0.0); +} + +BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & data) +{ + resetProcessingTime(); + stop_watch_.tic("total_time"); + + if (!root_lanelet_) { + root_lanelet_ = updateRootLanelet(data); + } + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); }); + + while (rclcpp::ok()) { + /** + * STEP1: get approved modules' output + */ + const auto approved_path_data = update(data); + + /** + * STEP2: check modules that need to be launched + */ + const auto candidate_module_opt = getCandidateModule(approved_path_data); + + /** + * STEP3: if there is no module that need to be launched, return approved modules' output + */ + if (!candidate_module_opt) { + candidate_module_opt_ = boost::none; + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return approved_path_data; + } + + /** + * STEP4: if there is module that should be launched, execute the module + */ + const auto name = candidate_module_opt.get()->name(); + stop_watch_.tic(name); + const auto result = run(candidate_module_opt.get(), data, approved_path_data); + processing_time_.at(name) += stop_watch_.toc(name, true); + + /** + * STEP5: if the candidate module's modification is NOT approved yet, return the result. + * NOTE: the result is output of the candidate module, but the output path don't contains path + * shape modification that needs approval. On the other hand, it could include velocity profile + * modification. + */ + if (candidate_module_opt.get()->isWaitingApproval()) { + candidate_module_opt_ = candidate_module_opt.get(); + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return result; + } + + /** + * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_ + */ + candidate_module_opt_ = boost::none; + addApprovedModule(candidate_module_opt.get()); + } + + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + + return {}; +} + +boost::optional PlannerManager::getCandidateModule( + const BehaviorModuleOutput & previous_module_output) const +{ + std::vector> request_modules{}; + + const auto block_simultaneous_execution = [this]() { + for (const auto & module_ptr : approved_module_ptrs_) { + const auto itr = std::find_if( + manager_ptrs_.begin(), manager_ptrs_.end(), + [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); }); + if (itr == manager_ptrs_.end()) { + return true; + } + if (!(*itr)->isSimultaneousExecutable()) { + return true; + } + } + return false; + }(); + + // pickup execution requested modules + for (const auto & manager_ptr : manager_ptrs_) { + stop_watch_.tic(manager_ptr->getModuleName()); + + const auto toc = [this, &manager_ptr]() { + const auto name = manager_ptr->getModuleName(); + processing_time_.at(name) += stop_watch_.toc(name, true); + }; + + // already exist the modules that don't support simultaneous execution. -> DO NOTHING. + if (block_simultaneous_execution) { + toc(); + continue; + } + + // the module doesn't support simultaneous execution. -> DO NOTHING. + if (!approved_module_ptrs_.empty() && !manager_ptr->isSimultaneousExecutable()) { + toc(); + continue; + } + + // check there is a launched module which is waiting approval as candidate. + if (!candidate_module_opt_) { + // launched module num reach limit. -> CAN'T LAUNCH NEW MODULE. DO NOTHING. + if (!manager_ptr->canLaunchNewModule()) { + toc(); + continue; + } + + // the module requests it to be launch. -> CAN LAUNCH THE MODULE. PUSH BACK AS REQUEST + // MODULES. + const auto new_module_ptr = manager_ptr->getNewModule(); + if (manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { + request_modules.emplace_back(manager_ptr, new_module_ptr); + } + + toc(); + continue; + } + + // candidate module already exist + const auto & name = candidate_module_opt_.get()->name(); + + // when the approved module throw another approval request, block all. -> CLEAR REQUEST MODULES + // AND PUSH BACK. + if (candidate_module_opt_.get()->isLockedNewModuleLaunch()) { + const auto itr = std::find_if( + manager_ptrs_.begin(), manager_ptrs_.end(), + [&name](const auto & m) { return m->getModuleName() == name; }); + if (itr == manager_ptrs_.end()) { + request_modules.clear(); + request_modules.emplace_back(*itr, candidate_module_opt_.get()); + toc(); + break; + } + } + + // same name module already launched as candidate. + if (name == manager_ptr->getModuleName()) { + // the module launched as candidate and is running now. the module hasn't thrown any approval + // yet. -> PUSH BACK AS REQUEST MODULES. + if (candidate_module_opt_.get()->getCurrentStatus() == ModuleStatus::RUNNING) { + request_modules.emplace_back(manager_ptr, candidate_module_opt_.get()); + } else { + // TODO(Satoshi OTA) this line is no longer needed? think later. + manager_ptr->deleteModules(candidate_module_opt_.get()); + } + + toc(); + continue; + } + + // different name module already launched as candidate. + // launched module num reach limit. -> CAN'T LAUNCH NEW MODULE. DO NOTHING. + if (!manager_ptr->canLaunchNewModule()) { + toc(); + continue; + } + + // the module requests it to be launch. -> CAN LAUNCH THE MODULE. PUSH BACK AS REQUEST MODULES. + const auto new_module_ptr = manager_ptr->getNewModule(); + if (!manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { + toc(); + continue; + } + + toc(); + request_modules.emplace_back(manager_ptr, new_module_ptr); + } + + // select one module to run as candidate module. + const auto high_priority_module = selectHighestPriorityModule(request_modules); + + if (!high_priority_module) { + return {}; + } + + // post process + { + const auto & manager = high_priority_module.get().first; + const auto & module_ptr = high_priority_module.get().second; + + // if the selected module is NOT registered in manager, registered the module. + if (!manager->exist(module_ptr)) { + manager->registerNewModule(module_ptr, previous_module_output); + } + + // if the current candidate module is NOT selected as high priority module, delete the candidate + // module from manager. + for (const auto & m : request_modules) { + if (m.first->getModuleName() != manager->getModuleName() && m.first->exist(m.second)) { + m.first->deleteModules(m.second); + } + } + } + + return high_priority_module.get().second; +} + +boost::optional> +PlannerManager::selectHighestPriorityModule( + std::vector> & request_modules) const +{ + if (request_modules.empty()) { + return {}; + } + + // TODO(someone) enhance this priority decision method. + std::sort(request_modules.begin(), request_modules.end(), [](auto a, auto b) { + return a.first->getPriority() < b.first->getPriority(); + }); + + return request_modules.front(); +} + +BehaviorModuleOutput PlannerManager::update(const std::shared_ptr & data) +{ + BehaviorModuleOutput output = getReferencePath(data); // generate root reference path. + + bool remove_after_module = false; + + for (auto itr = approved_module_ptrs_.begin(); itr != approved_module_ptrs_.end();) { + const auto & name = (*itr)->name(); + + stop_watch_.tic(name); + + // if one of the approved modules changes to waiting approval, remove all behind modules. + if (remove_after_module) { + itr = approved_module_ptrs_.erase(itr); + processing_time_.at(name) += stop_watch_.toc(name, true); + continue; + } + + const auto result = run(*itr, data, output); // execute approved module planning. + + // check the module is necessary or not. + if ((*itr)->getCurrentStatus() != ModuleStatus::RUNNING) { + if (itr == approved_module_ptrs_.begin()) { + // update root lanelet when the lane change is done. + if (name == "lane_change") { + root_lanelet_ = updateRootLanelet(data); + } + + deleteExpiredModules(*itr); // unregister the expired module from manager. + itr = approved_module_ptrs_.erase(itr); + output = result; + processing_time_.at(name) += stop_watch_.toc(name, true); + continue; + } + } + + // if one of the approved modules is waiting approval, the module is popped as candidate + // module again. + if ((*itr)->isWaitingApproval()) { + if (!!candidate_module_opt_) { + deleteExpiredModules(candidate_module_opt_.get()); + } + candidate_module_opt_ = *itr; + itr = approved_module_ptrs_.erase(itr); + remove_after_module = true; + processing_time_.at(name) += stop_watch_.toc(name, true); + continue; + } + + processing_time_.at(name) += stop_watch_.toc(name, true); + output = result; + itr++; + } + + return output; +} + +BehaviorModuleOutput PlannerManager::getReferencePath( + const std::shared_ptr & data) const +{ + PathWithLaneId reference_path{}; + + constexpr double extra_margin = 10.0; + + const auto & route_handler = data->route_handler; + const auto & pose = data->self_odometry->pose.pose; + const auto p = data->parameters; + + reference_path.header = route_handler->getRouteHeader(); + + const auto backward_length = + std::max(p.backward_path_length, p.backward_path_length + extra_margin); + + const auto lanelet_sequence = route_handler->getLaneletSequence( + root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); + + lanelet::ConstLanelet closest_lane{}; + if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { + return {}; + } + + const auto current_lanes = + route_handler->getLaneletSequence(closest_lane, pose, backward_length, p.forward_path_length); + + reference_path = util::getCenterLinePath( + *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); + + // clip backward length + const size_t current_seg_idx = data->findEgoSegmentIndex(reference_path.points); + util::clipPathLength( + reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length); + const auto drivable_lanelets = util::getLaneletsFromPath(reference_path, route_handler); + const auto drivable_lanes = util::generateDrivableLanes(drivable_lanelets); + + { + const int num_lane_change = + std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); + + const double lane_change_buffer = util::calcLaneChangeBuffer(p, num_lane_change); + + reference_path = util::setDecelerationVelocity( + *route_handler, reference_path, current_lanes, 2.0, lane_change_buffer); + } + + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); + + const auto expanded_lanes = util::expandLanelets(shorten_lanes, 0.0, 0.0); + + util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, data); + + BehaviorModuleOutput output; + output.path = std::make_shared(reference_path); + output.reference_path = std::make_shared(reference_path); + + return output; +} + +void PlannerManager::resetRootLanelet(const std::shared_ptr & data) +{ + if (!root_lanelet_) { + root_lanelet_ = updateRootLanelet(data); + return; + } + + const auto root_lanelet = updateRootLanelet(data); + + // check ego is in same lane + if (root_lanelet_.get().id() == root_lanelet.id()) { + return; + } + + const auto route_handler = data->route_handler; + const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.get()); + + // check ego is in next lane + for (const auto & next : next_lanelets) { + if (next.id() == root_lanelet_.get().id()) { + return; + } + } + + if (!approved_module_ptrs_.empty()) { + return; + } + + if (!!candidate_module_opt_) { + if (candidate_module_opt_.get()->isLockedNewModuleLaunch()) { + return; + } + } + + reset(); + + RCLCPP_INFO_EXPRESSION(logger_, verbose_, "change ego's following lane. reset."); +} + +void PlannerManager::print() const +{ + if (!verbose_) { + return; + } + + std::ostringstream string_stream; + string_stream << "\n"; + string_stream << "***********************************************************\n"; + string_stream << " planner manager status\n"; + string_stream << "-----------------------------------------------------------\n"; + string_stream << "registered modules: "; + for (const auto & m : manager_ptrs_) { + string_stream << "[" << m->getModuleName() << "]"; + } + + string_stream << "\n"; + string_stream << "approved modules : "; + for (const auto & m : approved_module_ptrs_) { + string_stream << "[" << m->name() << "]->"; + } + + string_stream << "\n"; + string_stream << "candidate module : "; + if (!!candidate_module_opt_) { + string_stream << "[" << candidate_module_opt_.get()->name() << "]"; + } + + string_stream << "\n" << std::fixed << std::setprecision(1); + string_stream << "processing time : "; + for (const auto & t : processing_time_) { + string_stream << std::right << "[" << std::setw(16) << std::left << t.first << ":" + << std::setw(4) << std::right << t.second << "ms]\n" + << std::setw(21); + } + + RCLCPP_INFO_STREAM(logger_, string_stream.str()); +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp index 42c8ae15919b1..c622fdc0532ea 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp +++ b/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp @@ -14,7 +14,11 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#ifdef USE_BEHAVIOR_TREE #include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#else +#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" +#endif namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 887e74bf393aa..04daf4053784e 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2464,4 +2464,27 @@ double calcLaneChangeBuffer( { return num_lane_change * calcTotalLaneChangeDistance(common_param) + length_to_intersection; } + +lanelet::ConstLanelets getLaneletsFromPath( + const PathWithLaneId & path, const std::shared_ptr & route_handler) +{ + std::vector unique_lanelet_ids; + for (const auto & p : path.points) { + const auto & lane_ids = p.lane_ids; + for (const auto & lane_id : lane_ids) { + if ( + std::find(unique_lanelet_ids.begin(), unique_lanelet_ids.end(), lane_id) == + unique_lanelet_ids.end()) { + unique_lanelet_ids.push_back(lane_id); + } + } + } + + lanelet::ConstLanelets lanelets; + for (const auto & lane_id : unique_lanelet_ids) { + lanelets.push_back(route_handler->getLaneletsFromId(lane_id)); + } + + return lanelets; +} } // namespace behavior_path_planner::util From 91653adc6593cd29d197f98512724e982c5f1b78 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 1 Mar 2023 10:01:09 +0900 Subject: [PATCH 05/24] refactor(obstacle_avoidance_planner): clean up the code (#2796) * update obstacle avoidance planner, static centerline optimizer, tier4_planning_launch Signed-off-by: Takayuki Murooka * update velocity on joint and correct trajectory z Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * minor change Signed-off-by: Takayuki Murooka * pre-commit Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../spline_interpolation_points_2d.hpp | 1 + .../src/spline_interpolation_points_2d.cpp | 12 +- .../motion_planning/motion_planning.launch.py | 4 +- .../obstacle_avoidance_planner/CMakeLists.txt | 38 +- planning/obstacle_avoidance_planner/README.md | 756 +----- .../obstacle_avoidance_planner.param.yaml | 248 +- .../{ => docs}/debug.md | 18 +- .../obstacle_avoidance_planner/docs/eb.md | 120 + .../obstacle_avoidance_planner/docs/mpt.md | 484 ++++ .../common_structs.hpp | 291 +-- .../debug_marker.hpp | 35 + .../eb_path_optimizer.hpp | 187 -- .../eb_path_smoother.hpp | 132 + .../mpt_optimizer.hpp | 396 +-- .../obstacle_avoidance_planner/node.hpp | 345 +-- .../replan_checker.hpp | 63 + .../state_equation_generator.hpp | 61 + .../obstacle_avoidance_planner/type_alias.hpp | 48 + .../utils/debug_utils.hpp | 47 - .../utils/geometry_utils.hpp | 71 + .../utils/trajectory_utils.hpp | 284 ++ .../utils/utils.hpp | 366 --- .../vehicle_model_bicycle_kinematics.hpp | 78 +- ...icle_model_bicycle_kinematics_no_delay.hpp | 93 - .../vehicle_model/vehicle_model_interface.hpp | 96 +- .../obstacle_avoidance_planner.launch.xml | 9 +- .../media/bounds.svg | 190 ++ .../obstacle_avoidance_planner/media/eb.svg | 371 +++ .../media/eb_constraint.svg | 991 +++++++ .../media/vehicle_circles.svg | 124 + .../obstacle_avoidance_planner/package.xml | 6 +- ...analyzer.py => calclation_time_plotter.py} | 31 +- .../scripts/trajectory_visualizer.py | 134 - .../src/debug_marker.cpp | 344 +++ .../src/eb_path_optimizer.cpp | 635 ----- .../src/eb_path_smoother.cpp | 416 +++ .../src/mpt_optimizer.cpp | 2294 ++++++++--------- .../obstacle_avoidance_planner/src/node.cpp | 1868 +++----------- .../src/replan_checker.cpp | 158 ++ .../src/state_equation_generator.cpp | 87 + .../src/utils/debug_utils.cpp | 858 ------ .../src/utils/geometry_utils.cpp | 205 ++ .../src/utils/trajectory_utils.cpp | 235 ++ .../src/utils/utils.cpp | 780 ------ .../vehicle_model_bicycle_kinematics.cpp | 50 +- ...icle_model_bicycle_kinematics_no_delay.cpp | 51 - .../vehicle_model/vehicle_model_interface.cpp | 25 +- .../CMakeLists.txt | 2 +- .../successive_trajectory_optimizer_node.hpp | 45 + .../static_centerline_optimizer.launch.xml | 6 +- .../rviz/static_centerline_optimizer.rviz | 76 +- .../src/static_centerline_optimizer_node.cpp | 7 +- .../successive_trajectory_optimizer_node.cpp | 109 + 53 files changed, 6831 insertions(+), 7550 deletions(-) rename planning/obstacle_avoidance_planner/{ => docs}/debug.md (91%) create mode 100644 planning/obstacle_avoidance_planner/docs/eb.md create mode 100644 planning/obstacle_avoidance_planner/docs/mpt.md create mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp create mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp create mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp create mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp create mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp create mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp create mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp create mode 100644 planning/obstacle_avoidance_planner/media/bounds.svg create mode 100644 planning/obstacle_avoidance_planner/media/eb.svg create mode 100644 planning/obstacle_avoidance_planner/media/eb_constraint.svg create mode 100644 planning/obstacle_avoidance_planner/media/vehicle_circles.svg rename planning/obstacle_avoidance_planner/scripts/{calclation_time_analyzer.py => calclation_time_plotter.py} (75%) delete mode 100755 planning/obstacle_avoidance_planner/scripts/trajectory_visualizer.py create mode 100644 planning/obstacle_avoidance_planner/src/debug_marker.cpp delete mode 100644 planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp create mode 100644 planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp create mode 100644 planning/obstacle_avoidance_planner/src/replan_checker.cpp create mode 100644 planning/obstacle_avoidance_planner/src/state_equation_generator.cpp delete mode 100644 planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp create mode 100644 planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp create mode 100644 planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp delete mode 100644 planning/obstacle_avoidance_planner/src/utils/utils.cpp delete mode 100644 planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp create mode 100644 planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp create mode 100644 planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index 567a1873aaa5c..653d6c61d05b6 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -84,6 +84,7 @@ class SplineInterpolationPoints2d void calcSplineCoefficientsInner(const std::vector & points); SplineInterpolation spline_x_; SplineInterpolation spline_y_; + SplineInterpolation spline_z_; std::vector base_s_vec_; }; diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 82c3841c424c5..848d65dfae575 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -35,12 +35,13 @@ std::vector calcEuclidDist(const std::vector & x, const std::vec return dist_v; } -std::array, 3> getBaseValues( +std::array, 4> getBaseValues( const std::vector & points) { // calculate x, y std::vector base_x; std::vector base_y; + std::vector base_z; for (size_t i = 0; i < points.size(); i++) { const auto & current_pos = points.at(i); if (i > 0) { @@ -53,16 +54,17 @@ std::array, 3> getBaseValues( } base_x.push_back(current_pos.x); base_y.push_back(current_pos.y); + base_z.push_back(current_pos.z); } // calculate base_keys, base_values - if (base_x.size() < 2 || base_y.size() < 2) { + if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) { throw std::logic_error("The numbef of unique points is not enough."); } const std::vector base_s = calcEuclidDist(base_x, base_y); - return {base_s, base_x, base_y}; + return {base_s, base_x, base_y, base_z}; } } // namespace @@ -137,10 +139,12 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0); const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0); + const double z = spline_z_.getSplineInterpolatedValues({whole_s}).at(0); geometry_msgs::msg::Point geom_point; geom_point.x = x; geom_point.y = y; + geom_point.z = z; return geom_point; } @@ -226,8 +230,10 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( base_s_vec_ = base.at(0); const auto & base_x_vec = base.at(1); const auto & base_y_vec = base.at(2); + const auto & base_z_vec = base.at(3); // calculate spline coefficients spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec); spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); + spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec); } diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index de8104af44576..03bf4479c8139 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -48,12 +48,12 @@ def launch_setup(context, *args, **kwargs): obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_avoidance_planner_component = ComposableNode( package="obstacle_avoidance_planner", - plugin="ObstacleAvoidancePlanner", + plugin="obstacle_avoidance_planner::ObstacleAvoidancePlanner", name="obstacle_avoidance_planner", namespace="", remappings=[ - ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/path", LaunchConfiguration("input_path_topic")), + ("~/input/odometry", "/localization/kinematic_state"), ("~/output/path", "obstacle_avoidance_planner/trajectory"), ], parameters=[ diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index b1b29a1a5f389..eb7c017af33d6 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -1,40 +1,52 @@ cmake_minimum_required(VERSION 3.14) project(obstacle_avoidance_planner) +include(CheckCXXCompilerFlag) + +# For Eigen vectorization. +check_cxx_compiler_flag("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE) +if(COMPILER_SUPPORTS_MARCH_NATIVE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") + message(STATUS "Enabling MARCH NATIVE ") +endif() + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED) ament_auto_add_library(obstacle_avoidance_planner SHARED + # node + src/node.cpp + # core algorithms + src/replan_checker.cpp + src/eb_path_smoother.cpp + src/mpt_optimizer.cpp + src/state_equation_generator.cpp + # debug marker + src/debug_marker.cpp + # vehicle model src/vehicle_model/vehicle_model_interface.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/eb_path_optimizer.cpp - src/mpt_optimizer.cpp - src/utils/utils.cpp - src/utils/debug_utils.cpp - src/node.cpp + # utils + src/utils/trajectory_utils.cpp + src/utils/geometry_utils.cpp ) target_include_directories(obstacle_avoidance_planner SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) -target_link_libraries(obstacle_avoidance_planner - ${OpenCV_LIBRARIES} -) - +# register node rclcpp_components_register_node(obstacle_avoidance_planner - PLUGIN "ObstacleAvoidancePlanner" + PLUGIN "obstacle_avoidance_planner::ObstacleAvoidancePlanner" EXECUTABLE obstacle_avoidance_planner_node ) ament_auto_package( INSTALL_TO_SHARE launch - config scripts + config ) diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/obstacle_avoidance_planner/README.md index 461a657d14a8d..1a03b695aa699 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/obstacle_avoidance_planner/README.md @@ -1,32 +1,33 @@ ## Purpose -This package generates a trajectory that is feasible to drive and collision free based on a reference path, drivable area, and static/dynamic obstacles. -Only position and orientation of trajectory are calculated in this module (velocity is just aligned from the one in the path), and velocity or acceleration will be updated in the latter modules. +This package generates a trajectory that is kinematically-feasible to drive and collision-free based on the input path, drivable area. +Only position and orientation of trajectory are updated in this module, and velocity is just taken over from the one in the input path. ## Feature This package is able to -- follow the behavior path smoothly +- make the trajectory smooth - make the trajectory inside the drivable area as much as possible -- insert stop point if its trajectory point is outside the drivable area + - NOTE: Static obstacles to avoid can be removed from the drivable area. +- insert stop point before the planned footprint will be outside the drivable area + +Note that the velocity is just taken over from the input path. ## Inputs / Outputs ### input -| Name | Type | Description | -| ---------------------------------------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/Path | Reference path and the corresponding drivable area | -| `~/input/objects` | autoware_auto_perception_msgs/PredictedObjects | Recognized objects around the vehicle | -| `/localization/kinematic_kinematics` | nav_msgs/Odometry | Current Velocity of ego vehicle | -| `/planning/scenario_planning/lane_driving/obstacle_avoidance_approval` | tier4_planning_msgs/EnableAvoidance | Approval to execute obstacle avoidance | +| Name | Type | Description | +| ------------------ | ------------------------------------ | -------------------------------------------------- | +| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current Velocity of ego vehicle | ### output -| Name | Type | Description | -| --------------------- | -------------------------------------- | ----------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/Trajectory | Optimized trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | Optimized trajectory that is feasible to drive and collision-free | ## Flowchart @@ -37,88 +38,81 @@ Flowchart of functions is explained here. title pathCallback start +:isDataReady; + +:createPlannerData; + group generateOptimizedTrajectory - :checkReplan; - if (replanning required?) then (yes) - group getMaps - :getDrivableArea; - :getRoadClearanceMap; - :drawObstacleOnImage; - :getObstacleClearanceMap; - end group - - group optimizeTrajectory + group optimizeTrajectory + :check replan; + if (replanning required?) then (yes) :getEBTrajectory; :getModelPredictiveTrajectory; - if (optimization failed?) then (no) else (yes) :send previous\n trajectory; endif - end group - - :insertZeroVelocityOutsideDrivableArea; - - :publishDebugDataInOptimization; - else (no) - :send previous\n trajectory; - endif + else (no) + :send previous\n trajectory; + endif + end group + + :applyInputVelocity; + :insertZeroVelocityOutsideDrivableArea; + :publishDebugMarkerOfOptimization; end group -group generatePostProcessedTrajectory - :getExtendedOptimizedTrajectory; - :concatTrajectory; - :generateFineTrajectoryPoints; - :alignVelocity; -end group +:extendTrajectory; -:convertToTrajectory; +:setZeroVelocityAfterStopPoint; -:publishDebugDataInMain; +:publishDebugData; stop @enduml ``` -### checkReplan +### createPlannerData -When one of the following conditions are realized, callback function to generate a trajectory is called and publish the trajectory. -Otherwise, previously generated (optimized) trajectory is published just with aligning the velocity from the latest behavior path. +The following data for planning is created. -- Ego moves a certain distance compared to the previous ego pose (default: 3.0 [m]) -- Time passes (default: 1.0 [s]) -- Ego is far from the previously generated trajectory +```cpp +struct PlannerData +{ + // input + Header header; + std::vector traj_points; // converted from the input path + std::vector left_bound; + std::vector right_bound; -### getRoadClearanceMap - -Cost map is generated according to the distance to the road boundaries. - -These cost maps are used in the optimization to generate a collision-free trajectory. - -### drawObstacleOnImage - -Only obstacles that are static and locate in a shoulder lane is decided to avoid. -In detail, this equals to the following three conditions at the same time, and the red obstacles in the figure (id: 3, 4, 5) is to be avoided. + // ego + geometry_msgs::msg::Pose ego_pose; + double ego_vel; +}; +``` -- Velocity is under a certain value (default: 0.1 [m/s]) -- CoG of the obstacles is not on the center line - - so that the ego will not avoid the car in front of the ego in the same lane. -- At least one point of the obstacle polygon is outside the drivable area. +### check replan -![obstacle_to_avoid](./media/obstacle_to_avoid.drawio.svg) +When one of the following conditions are met, trajectory optimization will be executed. +Otherwise, previously optimized trajectory is used with updating the velocity from the latest input path. -### getObstacleClearanceMap +max_path_shape_around_ego_lat_dist -Cost map is generated according to the distance to the target obstacles to be avoided. +- Ego moves longer than `replan.max_ego_moving_dist` in one cycle. (default: 3.0 [m]) + - This is for when the ego pose is set again in the simulation. +- Trajectory's end, which is considered as the goal pose, moves longer than `replan.max_goal_moving_dist` in one cycle. (default: 15.0 [ms]) + - When the goal pose is set again, the planning should be reset. +- Time passes. (default: 1.0 [s]) + - The optimization is skipped for a while sine the optimization is sometimes heavy. +- The input path changes laterally longer than `replan.max_path_shape_around_ego_lat_dist` in one cycle. (default: 2.0) ### getEBTrajectory -The latter optimization (MPT) assumes that the reference path is smooth enough. -Therefore the path from behavior is made smooth here, and send to the optimization as a format of trajectory. -Obstacles are ignored in this function. +The latter optimization (model predictive trajectory) assumes that the reference path is smooth enough. +This function makes the input path smooth by elastic band. -More details can be seen in the Elastic Band section. +More details about elastic band can be seen [here](docs/eb.md). ### getModelPredictiveTrajectory @@ -136,7 +130,7 @@ Some examples are shown in the following figure, and it is shown that the trajec ![mpt_optimization_offset](./media/mpt_optimization_offset.svg) -More details can be seen in the Model Predictive Trajectory section. +More details can be seen [here](docs/mpt.md). ### insertZeroVelocityOutsideDrivableArea @@ -166,561 +160,35 @@ Therefore, we have to make sure that the optimized trajectory is inside the driv Velocity is assigned in the result trajectory from the velocity in the behavior path. The shapes of the trajectory and the path are different, therefore the each nearest trajectory point to the path is searched and interpolated linearly. -## Algorithms - -In this section, Elastic band (to make the path smooth) and Model Predictive Trajectory (to make the trajectory kinematically feasible and collision-free) will be explained in detail. - -### Elastic band - -#### Abstract - -Elastic band smooths the path generated in the behavior. -Since the latter process of optimization uses the curvature and normal vector of the reference path, smoothing should be applied here so that the optimization will be stable. - -This smoothing process does not consider collision. -Therefore the output path may have a collision with road boundaries or obstacles. - -#### Formulation - -We formulate a QP problem minimizing the distance between the previous point and the next point for each point. -Conditions that each point can move to a certain extent are used so that the path will not changed a lot but will be smoother. - -For $k$'th point ($\boldsymbol{p}_k = (x_k, y_k)$), the objective function is as follows. -The beginning and end point are fixed during the optimization. - -$$ -\begin{align} -\ J & = \min \sum_{k=1}^{n-1} ||(\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}) - (\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1})||^2 \\ -\ & = \min \sum_{k=1}^{n-1} ||\boldsymbol{p}_{k+1} - 2 \boldsymbol{p}_{k} + \boldsymbol{p}_{k-1}||^2 \\ -\ & = \min \sum_{k=1}^{n-1} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\ -\ & = \min - \begin{pmatrix} - \ x_0 \\ - \ x_1 \\ - \ x_2 \\ - \vdots \\ - \ x_{n-2}\\ - \ x_{n-1} \\ - \ x_{n} \\ - \ y_0 \\ - \ y_1 \\ - \ y_2 \\ - \vdots \\ - \ y_{n-2}\\ - \ y_{n-1} \\ - \ y_{n} \\ - \end{pmatrix}^T - \begin{pmatrix} - 1 & -2 & 1 & 0 & \dots& \\ - -2 & 5 & -4 & 1 & 0 &\dots \\ - 1 & -4 & 6 & -4 & 1 & \\ - 0 & 1 & -4 & 6 & -4 & \\ - \vdots & 0 & \ddots&\ddots& \ddots \\ - & \vdots & & & \\ - & & & 1 & -4 & 6 & -4 & 1 \\ - & & & & 1 & -4 & 5 & -2 \\ - & & & & & 1 & -2 & 1& \\ - & & & & & & & &1 & -2 & 1 & 0 & \dots& \\ - & & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\ - & & & & & & & &1 & -4 & 6 & -4 & 1 & \\ - & & & & & & & &0 & 1 & -4 & 6 & -4 & \\ - & & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\ - & & & & & & & & & \vdots & & & \\ - & & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\ - & & & & & & & & & & & & 1 & -4 & 5 & -2 \\ - & & & & & & & & & & & & & 1 & -2 & 1& \\ - \end{pmatrix} - \begin{pmatrix} - \ x_0 \\ - \ x_1 \\ - \ x_2 \\ - \vdots \\ - \ x_{n-2}\\ - \ x_{n-1} \\ - \ x_{n} \\ - \ y_0 \\ - \ y_1 \\ - \ y_2 \\ - \vdots \\ - \ y_{n-2}\\ - \ y_{n-1} \\ - \ y_{n} \\ - \end{pmatrix} -\end{align} -$$ - -### Model predictive trajectory - -#### Abstract - -Model Predictive Trajectory (MPT) calculates the trajectory that realizes the following conditions. - -- Kinematically feasible for linear vehicle kinematics model -- Collision free with obstacles and road boundaries - -Conditions for collision free is considered to be not hard constraints but soft constraints. -When the optimization failed or the optimized trajectory is not collision free, the output trajectory will be previously generated trajectory. - -Trajectory near the ego must be stable, therefore the condition where trajectory points near the ego are the same as previously generated trajectory is considered, and this is the only hard constraints in MPT. - -#### Vehicle kinematics - -As the following figure, we consider the bicycle kinematics model in the frenet frame to track the reference path. -At time step $k$, we define lateral distance to the reference path, heading angle against the reference path, and steer angle as $y_k$, $\theta_k$, and $\delta_k$ respectively. - -![vehicle_error_kinematics](./media/vehicle_error_kinematics.png) - -Assuming that the commanded steer angle is $\delta_{des, k}$, the kinematics model in the frenet frame is formulated as follows. -We also assume that the steer angle $\delta_k$ is first-order lag to the commanded one. - -$$ -\begin{align} -y_{k+1} & = y_{k} + v \sin \theta_k dt \\ -\theta_{k+1} & = \theta_k + \frac{v \tan \delta_k}{L}dt - \kappa_k v \cos \theta_k dt \\ -\delta_{k+1} & = \delta_k - \frac{\delta_k - \delta_{des,k}}{\tau}dt -\end{align} -$$ - -##### Linearization - -Then we linearize these equations. -$y_k$ and $\theta_k$ are tracking errors, so we assume that those are small enough. -Therefore $\sin \theta_k \approx \theta_k$. - -Since $\delta_k$ is a steer angle, it is not always small. -By using a reference steer angle $\delta_{\mathrm{ref}, k}$ calculated by the reference path curvature $\kappa_k$, we express $\delta_k$ with a small value $\Delta \delta_k$. - -Note that the steer angle $\delta_k$ is within the steer angle limitation $\delta_{\max}$. -When the reference steer angle $\delta_{\mathrm{ref}, k}$ is larger than the steer angle limitation $\delta_{\max}$, and $\delta_{\mathrm{ref}, k}$ is used to linearize the steer angle, $\Delta \delta_k$ is $\Delta \delta_k = \delta - \delta_{\mathrm{ref}, k} = \delta_{\max} - \delta_{\mathrm{ref}, k}$, and the absolute $\Delta \delta_k$ gets larger. -Therefore, we have to apply the steer angle limitation to $\delta_{\mathrm{ref}, k}$ as well. - -$$ -\begin{align} -\delta_{\mathrm{ref}, k} & = \mathrm{clamp}(\arctan(L \kappa_k), -\delta_{\max}, \delta_{\max}) \\ -\delta_k & = \delta_{\mathrm{ref}, k} + \Delta \delta_k, \ \Delta \delta_k \ll 1 \\ -\end{align} -$$ - -$\mathrm{clamp}(v, v_{\min}, v_{\max})$ is a function to convert $v$ to be larger than $v_{\min}$ and smaller than $v_{\max}$. - -Using this $\delta_{\mathrm{ref}, k}$, $\tan \delta_k$ is linearized as follows. - -$$ -\begin{align} -\tan \delta_k & \approx \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} \Delta \delta_k \\ -& = \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} (\delta_{\mathrm{ref}, k} - \delta_k) \\ -& = \tan \delta_{\mathrm{ref}, k} - \frac{\delta_{\mathrm{ref}, k}}{\cos^2 \delta_{\mathrm{ref}, k}} + \frac{1}{\cos^2 \delta_{\mathrm{ref}, k}} \delta_k -\end{align} -$$ - -##### One-step state equation - -Based on the linearization, the error kinematics is formulated with the following linear equations, - -$$ -\begin{align} - \begin{pmatrix} - y_{k+1} \\ - \theta_{k+1} - \end{pmatrix} - = - \begin{pmatrix} - 1 & v dt \\ - 0 & 1 \\ - \end{pmatrix} - \begin{pmatrix} - y_k \\ - \theta_k \\ - \end{pmatrix} - + - \begin{pmatrix} - 0 \\ - \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ - \end{pmatrix} - \delta_{k} - + - \begin{pmatrix} - 0 \\ - \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt\\ - \end{pmatrix} -\end{align} -$$ - -which can be formulated as follows with the state $\boldsymbol{x}$, control input $u$ and some matrices, where $\boldsymbol{x} = (y_k, \theta_k)$ - -$$ -\begin{align} - \boldsymbol{x}_{k+1} = A_k \boldsymbol{x}_k + \boldsymbol{b}_k u_k + \boldsymbol{w}_k -\end{align} -$$ - -##### Time-series state equation - -Then, we formulate time-series state equation by concatenating states, control inputs and matrices respectively as - -$$ -\begin{align} - \boldsymbol{x} = A \boldsymbol{x}_0 + B \boldsymbol{u} + \boldsymbol{w} -\end{align} -$$ - -where - -$$ -\begin{align} -\boldsymbol{x} = (\boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \boldsymbol{x}^T_3, \dots, \boldsymbol{x}^T_{n-1})^T \\ -\boldsymbol{u} = (u_0, u_1, u_2, \dots, u_{n-2})^T \\ -\boldsymbol{w} = (\boldsymbol{w}^T_0, \boldsymbol{w}^T_1, \boldsymbol{w}^T_2, \dots, \boldsymbol{w}^T_{n-1})^T. \\ -\end{align} -$$ - -In detail, each matrices are constructed as follows. - -$$ -\begin{align} - \begin{pmatrix} - \boldsymbol{x}_1 \\ - \boldsymbol{x}_2 \\ - \boldsymbol{x}_3 \\ - \vdots \\ - \boldsymbol{x}_{n-1} - \end{pmatrix} - = - \begin{pmatrix} - A_0 \\ - A_1 A_0 \\ - A_2 A_1 A_0\\ - \vdots \\ - \prod\limits_{k=0}^{n-1} A_{k} - \end{pmatrix} - \boldsymbol{x}_0 - + - \begin{pmatrix} - B_0 & 0 & & \dots & 0 \\ - A_0 B_0 & B_1 & 0 & \dots & 0 \\ - A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} - \end{pmatrix} - \begin{pmatrix} - u_0 \\ - u_1 \\ - u_2 \\ - \vdots \\ - u_{n-2} - \end{pmatrix} - + - \begin{pmatrix} - I & 0 & & \dots & 0 \\ - A_0 & I & 0 & \dots & 0 \\ - A_1 A_0 & A_0 & I & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I - \end{pmatrix} - \begin{pmatrix} - \boldsymbol{w}_0 \\ - \boldsymbol{w}_1 \\ - \boldsymbol{w}_2 \\ - \vdots \\ - \boldsymbol{w}_{n-2} - \end{pmatrix} -\end{align} -$$ - -##### Free-boundary-conditioned time-series state equation - -For path planning which does not start from the current ego pose, $\boldsymbol{x}_0$ should be the design variable of optimization. -Therefore, we make $\boldsymbol{u}'$ by concatenating $\boldsymbol{x}_0$ and $\boldsymbol{u}$, and redefine $\boldsymbol{x}$ as follows. - -$$ -\begin{align} - \boldsymbol{u}' & = (\boldsymbol{x}^T_0, \boldsymbol{u}^T)^T \\ - \boldsymbol{x} & = (\boldsymbol{x}^T_0, \boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \dots, \boldsymbol{x}^T_{n-1})^T -\end{align} -$$ - -Then we get the following state equation - -$$ -\begin{align} - \boldsymbol{x}' = B \boldsymbol{u}' + \boldsymbol{w}, -\end{align} -$$ - -which is in detail - -$$ -\begin{align} - \begin{pmatrix} - \boldsymbol{x}_0 \\ - \boldsymbol{x}_1 \\ - \boldsymbol{x}_2 \\ - \boldsymbol{x}_3 \\ - \vdots \\ - \boldsymbol{x}_{n-1} - \end{pmatrix} - = - \begin{pmatrix} - I & 0 & \dots & & & 0 \\ - A_0 & B_0 & 0 & & \dots & 0 \\ - A_1 A_0 & A_0 B_0 & B_1 & 0 & \dots & 0 \\ - A_2 A_1 A_0 & A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ - \vdots & \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-1} A_k & \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} - \end{pmatrix} - \begin{pmatrix} - \boldsymbol{x}_0 \\ - u_0 \\ - u_1 \\ - u_2 \\ - \vdots \\ - u_{n-2} - \end{pmatrix} - + - \begin{pmatrix} - 0 & \dots & & & 0 \\ - I & 0 & & \dots & 0 \\ - A_0 & I & 0 & \dots & 0 \\ - A_1 A_0 & A_0 & I & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ - \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I - \end{pmatrix} - \begin{pmatrix} - \boldsymbol{w}_0 \\ - \boldsymbol{w}_1 \\ - \boldsymbol{w}_2 \\ - \vdots \\ - \boldsymbol{w}_{n-2} - \end{pmatrix}. -\end{align} -$$ - -#### Objective function - -The objective function for smoothing and tracking is shown as follows, which can be formulated with value function matrices $Q, R$. - -$$ -\begin{align} -J_1 (\boldsymbol{x}', \boldsymbol{u}') & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 \\ -& = \boldsymbol{x}'^T Q \boldsymbol{x}' + \boldsymbol{u}'^T R \boldsymbol{u}' \\ -& = \boldsymbol{u}'^T H \boldsymbol{u}' + \boldsymbol{u}'^T \boldsymbol{f} -\end{align} -$$ - -As mentioned before, the constraints to be collision free with obstacles and road boundaries are formulated to be soft constraints. -Assuming that the lateral distance to the road boundaries or obstacles from the back wheel center, front wheel center, and the point between them are $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ respectively, and slack variables for each point are $\lambda_{\mathrm{base}}, \lambda_{\mathrm{top}}, \lambda_{\mathrm{mid}}$, the soft constraints can be formulated as follows. - -$$ -y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\ -y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\ -y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} \\ -0 \leq \lambda_{\mathrm{base}, k} \\ -0 \leq \lambda_{\mathrm{top}, k} \\ -0 \leq \lambda_{\mathrm{mid}, k} -$$ - -Since $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ is formulated as a linear function of $y_k$, the objective function for soft constraints is formulated as follows. - -$$ -\begin{align} -J_2 & (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol {\lambda}_\mathrm{mid})\\ -& = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k} + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k} + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k} -\end{align} -$$ - -Slack variables are also design variables for optimization. -We define a vector $\boldsymbol{v}$, that concatenates all the design variables. - -$$ -\begin{align} -\boldsymbol{v} = -\begin{pmatrix} - \boldsymbol{u}'^T & \boldsymbol{\lambda}_\mathrm{base}^T & \boldsymbol{\lambda}_\mathrm{top}^T & \boldsymbol{\lambda}_\mathrm{mid}^T -\end{pmatrix}^T -\end{align} -$$ - -The summation of these two objective functions is the objective function for the optimization problem. - -$$ -\begin{align} -\min_{\boldsymbol{v}} J (\boldsymbol{v}) = \min_{\boldsymbol{v}} J_1 (\boldsymbol{u}') + J_2 (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol{\lambda}_\mathrm{mid}) -\end{align} -$$ - -As mentioned before, we use hard constraints where some trajectory points in front of the ego are the same as the previously generated trajectory points. -This hard constraints is formulated as follows. - -$$ -\begin{align} -\delta_k = \delta_{k}^{\mathrm{prev}} (0 \leq i \leq N_{\mathrm{fix}}) -\end{align} -$$ - -Finally we transform those objective functions to the following QP problem, and solve it. - -$$ -\begin{align} -\min_{\boldsymbol{v}} \ & \frac{1}{2} \boldsymbol{v}^T \boldsymbol{H} \boldsymbol{v} + \boldsymbol{f} \boldsymbol{v} \\ -\mathrm{s.t.} \ & \boldsymbol{b}_{lower} \leq \boldsymbol{A} \boldsymbol{v} \leq \boldsymbol{b}_{upper} -\end{align} -$$ - -#### Constraints - -##### Steer angle limitation - -Steer angle has a certain limitation ($\delta_{max}$, $\delta_{min}$). -Therefore we add linear inequality equations. - -$$ -\begin{align} -\delta_{min} \leq \delta_i \leq \delta_{max} -\end{align} -$$ - -##### Collision free - -To realize collision-free path planning, we have to formulate constraints that the vehicle is inside the road (moreover, a certain meter far from the road boundary) and does not collide with obstacles in linear equations. -For linearity, we chose a method to approximate the vehicle shape with a set of circles, that is reliable and easy to implement. - -Now we formulate the linear constraints where a set of circles on each trajectory point is collision-free. -For collision checking, we have a drivable area in the format of an image where walls or obstacles are filled with a color. -By using this drivable area, we calculate upper (left) and lower (right) boundaries along reference points so that we can interpolate boundaries on any position on the trajectory. - -Assuming that upper and lower boundaries are $b_l$, $b_u$ respectively, and $r$ is a radius of a circle, lateral deviation of the circle center $y'$ has to be - -$$ -b_l + r \leq y' \leq b_u - r. -$$ - -Based on the following figure, $y'$ can be formulated as follows. - -$$ -\begin{align} -y' & = L \sin(\theta + \beta) + y \cos \beta - l \sin(\gamma - \phi_a) \\ -& = L \sin \theta \cos \beta + L \cos \theta \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \\ -& \approx L \theta \cos \beta + L \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) -\end{align} -$$ - -$$ -b_l + r - \lambda \leq y' \leq b_u - r + \lambda. -$$ - -$$ -\begin{align} -y' & = C_1 \boldsymbol{x} + C_2 \\ -& = C_1 (B \boldsymbol{v} + \boldsymbol{w}) + C_2 \\ -& = C_1 B \boldsymbol{v} + \boldsymbol{w} + C_2 -\end{align} -$$ - -Note that longitudinal position of the circle center and the trajectory point to calculate boundaries are different. -But each boundaries are vertical against the trajectory, resulting in less distortion by the longitudinal position difference since road boundaries does not change so much. -For example, if the boundaries are not vertical against the trajectory and there is a certain difference of longitudinal position between the circe center and the trajectory point, we can easily guess that there is much more distortion when comparing lateral deviation and boundaries. - -$$ -\begin{align} - A_{blk} & = - \begin{pmatrix} - C_1 B & O & \dots & O & I_{N_{ref} \times N_{ref}} & O \dots & O\\ - -C_1 B & O & \dots & O & I & O \dots & O\\ - O & O & \dots & O & I & O \dots & O - \end{pmatrix} - \in \boldsymbol{R}^{3 N_{ref} \times D_v + N_{circle} N_{ref}} \\ - \boldsymbol{b}_{lower, blk} & = - \begin{pmatrix} - \boldsymbol{b}_{lower} - C_1 \boldsymbol{w} - C_2 \\ - -\boldsymbol{b}_{upper} + C_1 \boldsymbol{w} + C_2 \\ - O - \end{pmatrix} - \in \boldsymbol{R}^{3 N_{ref}} \\ - \boldsymbol{b}_{upper, blk} & = \boldsymbol{\infty} - \in \boldsymbol{R}^{3 N_{ref}} -\end{align} -$$ - -We will explain options for optimization. - -###### L-infinity optimization - -The above formulation is called L2 norm for slack variables. -Instead, if we use L-infinity norm where slack variables are shared by enabling `l_inf_norm`. - -$$ -\begin{align} - A_{blk} = - \begin{pmatrix} - C_1 B & I_{N_{ref} \times N_{ref}} \\ - -C_1 B & I \\ - O & I - \end{pmatrix} -\in \boldsymbol{R}^{3N_{ref} \times D_v + N_{ref}} -\end{align} -$$ - -###### Two-step soft constraints - -$$ -\begin{align} -\boldsymbol{v}' = - \begin{pmatrix} - \boldsymbol{v} \\ - \boldsymbol{\lambda}^{soft_1} \\ - \boldsymbol{\lambda}^{soft_2} \\ - \end{pmatrix} - \in \boldsymbol{R}^{D_v + 2N_{slack}} -\end{align} -$$ - -$*$ depends on whether to use L2 norm or L-infinity optimization. - -$$ -\begin{align} - A_{blk} & = - \begin{pmatrix} - A^{soft_1}_{blk} \\ - A^{soft_2}_{blk} \\ - \end{pmatrix}\\ - & = - \begin{pmatrix} - C_1^{soft_1} B & & \\ - -C_1^{soft_1} B & \Huge{*} & \Huge{O} \\ - O & & \\ - C_1^{soft_2} B & & \\ - -C_1^{soft_2} B & \Huge{O} & \Huge{*} \\ - O & & - \end{pmatrix} - \in \boldsymbol{R}^{6 N_{ref} \times D_v + 2 N_{slack}} -\end{align} -$$ - -$N_{slack}$ is $N_{circle}$ when L2 optimization, or $1$ when L-infinity optimization. -$N_{circle}$ is the number of circles to check collision. - -## Tuning - -### Vehicle - -- max steering wheel degree - - `mpt.kinematics.max_steer_deg` - -### Boundary search - -- `advanced.mpt.bounds_search_widths` - - In order to efficiently search precise lateral boundaries on each trajectory point, different resolutions of search widths are defined. - - By default, [0.45, 0.15, 0.05, 0.01] is used. In this case, the goal is to get the boundaries' length on each trajectory point with 0.01 [m] resolution. - - Firstly, lateral boundaries are searched with a rough resolution (= 0.45 [m]). - - Then, within its 0.45 [m] resolution which boundaries are inside, they are searched again with a bit precise resolution (= 0.15 [m]). - - Following this rule, finally boundaries with 0.01 [m] will be found. - -### Assumptions - -- EB optimized trajectory length should be longer than MPT optimized trajectory length - - since MPT result may be jerky because of non-fixed reference path (= EB optimized trajectory) - - At least, EB fixed optimized trajectory length must be longer than MPT fixed optimization trajectory length - - This causes the case that there is a large difference between MPT fixed optimized point and MPT optimized point just after the point. +## Limitation + +- Computation cost is sometimes high. +- Because of the approximation such as linearization, some narrow roads cannot be run by the planner. +- Roles of planning for `behavior_path_planner` and `obstacle_avoidance_planner` are not decided clearly. Both can avoid obstacles. + +## Comparison to other methods + +Trajectory planning problem that satisfies kinematically-feasibility and collision-free has two main characteristics that makes hard to be solved: one is non-convex and the other is high dimension. +Based on the characteristics, we investigate pros/cons of the typical planning methods: optimization-based, sampling-based, and learning-based method. + +### Optimization-based method + +- pros: comparatively fast against high dimension by leveraging the gradient descent +- cons: often converge to the local minima in the non-convex problem + +### Sampling-based method + +- pros: realize global optimization +- cons: high computation cost especially in the complex case + +### Learning-based method + +- under research yet + +Based on these pros/cons, we chose the optimization-based planner first. +Although it has a cons to converge to the local minima, it can get a good solution by the preprocessing to approximate the problem to convex that almost equals to the original non-convex problem. + +## How to Tune Parameters ### Drivability in narrow roads @@ -737,7 +205,7 @@ $N_{circle}$ is the number of circles to check collision. - Loose EB optimization - - 1. make `eb.common.delta_arc_length_for_eb` large and `eb.common.num_sampling_points_for_eb` small + - 1. make `eb.common.delta_arc_length` large and `eb.common.num_points` small - This makes the number of design variables smaller - Be careful about the trajectory length between MPT and EB as shown in Assumptions. - However, empirically this causes large turn at the corner (e.g. The vehicle turns a steering wheel to the opposite side (=left) a bit just before the corner turning to right) @@ -770,44 +238,16 @@ $N_{circle}$ is the number of circles to check collision. ### Other options -- `option.skip_optimization` skips EB and MPT optimization. -- `option.enable_pre_smoothing` enables EB which is smoothing the trajectory for MPT. +- `option.enable_skip_optimization` skips EB and MPT optimization. +- `option.enable_smoothing` enables EB which is smoothing the trajectory for MPT. - EB is not required if the reference path for MPT is smooth enough and does not change its shape suddenly -- `option.is_showing_calculation_time` enables showing each calculation time for functions and total calculation time on the terminal. -- `option.is_stopping_if_outside_drivable_area` enables stopping just before the generated trajectory point will be outside the drivable area. +- `option.enable_calculation_time_info` enables showing each calculation time for functions and total calculation time on the terminal. +- `option.enable_outside_drivable_area_stop` enables stopping just before the generated trajectory point will be outside the drivable area. - `mpt.option.plan_from_ego` enables planning from the ego pose when the ego's velocity is zero. - `mpt.option.max_plan_from_ego_length` maximum length threshold to plan from ego. it is enabled when the length of trajectory is shorter than this value. - `mpt.option.two_step_soft_constraint` enables two step of soft constraints for collision free - `mpt.option.soft_clearance_from_road` and `mpt.option.soft_second_clearance_from_road` are the weight. -## Limitation - -- When turning right or left in the intersection, the output trajectory is close to the outside road boundary. -- Roles of planning for behavior_path_planner and obstacle_avoidance_planner are not decided clearly. -- High computation cost - -## Comparison to other methods - -Planning a trajectory that satisfies kinematic feasibility and collision-free has two main characteristics that makes hard to be solved: one is non-convex and the other is high dimension. -According to the characteristics, we investigate pros and cons of the typical planning methods: optimization-based, sampling-based, and learning-based method. - -### Optimization-based method - -- pros: comparatively fast against high dimension by leveraging the gradient descent -- cons: often converge to the local minima in the non-convex problem - -### Sampling-based method - -- pros: realize global optimization -- cons: high computation cost especially in the complex case - -### Learning-based method - -under research yet - -Based on these pros/cons, we chose the optimization-based planner first. -Although it has a cons to converge to the local minima, it can get a good solution by the preprocessing to approximate the convex problem that almost equals to the original non-convex problem. - -# Debug +## How To Debug -Debug information are written [here](debug.md). +How to debug can be seen [here](docs/debug.md). diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index c2aab909e8b6c..334da22dc27c0 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -1,166 +1,140 @@ /**: ros__parameters: option: - # publish - is_publishing_debug_visualization_marker: true - is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid + enable_smoothing: true # enable path smoothing by elastic band + enable_skip_optimization: false # skip elastic band and model predictive trajectory + enable_reset_prev_optimization: false # If true, optimization has no fix constraint to the previous result. + enable_outside_drivable_area_stop: true # stop if the ego's trajectory footprint is outside the drivable area + use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered. - is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area + debug: + # flag to publish + enable_pub_debug_marker: true # publish debug marker - # show - is_showing_debug_info: false - is_showing_calculation_time: false - - # other - enable_avoidance: false # enable avoidance function - enable_pre_smoothing: true # enable EB - skip_optimization: false # skip MPT and EB - reset_prev_optimization: false - is_considering_footprint_edges: false # consider ego footprint edges to decide whether footprint is outside drivable area + # flag to show + enable_debug_info: false + enable_calculation_time_info: false common: - # sampling - num_sampling_points: 100 # number of optimizing points - - # trajectory total/fixing length - trajectory_length: 300.0 # total trajectory length[m] - - forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] - forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] + # output + output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] + output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] - backward_fixing_distance: 5.0 # backward fixing length from base_link [m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] + # replanning & trimming trajectory param outside algorithm + replan: + max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] + # make max_goal_moving_dist long to keep start point fixed for pull over + max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] + + # eb param + eb: + option: + enable_warm_start: true + enable_optimization_validation: false - delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] - delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point - delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + common: + num_points: 100 # number of points for optimization [-] + delta_arc_length: 1.0 # delta arc length for optimization [m] - num_fix_points_for_extending: 50 # number of fixing points when extending - max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] + clearance: + num_joint_points: 3 # number of joint points (joint means connecting fixing and smoothing) - enable_clipping_fixed_traj: false - non_fixed_trajectory_length: 5.0 # length of the trajectory merging optimized mpt trajectory to original(not optimized) trajectory + clearance_for_fix: 0.0 # maximum optimizing range when applying fixing + clearance_for_joint: 0.1 # maximum optimizing range when applying jointing + clearance_for_smooth: 0.1 # maximum optimizing range when applying smoothing - vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . - # This margin will be realized with delta_arc_length_for_mpt_points m precision. + weight: + smooth_weight: 1.0 + lat_error_weight: 0.001 - object: # avoiding object - max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] - max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] + qp: + max_iteration: 10000 # max iteration when solving QP + eps_abs: 1.0e-7 # eps abs when solving OSQP + eps_rel: 1.0e-7 # eps rel when solving OSQP - avoiding_object_type: - unknown: true - car: true - truck: true - bus: true - bicycle: true - motorbike: true - pedestrian: true - animal: true + validation: # if enable_optimization_validation is true + max_error: 3.0 # [m] # mpt param mpt: option: - steer_limit_constraint: true - fix_points_around_ego: true - plan_from_ego: true - max_plan_from_ego_length: 10.0 + # TODO(murooka) enable the following. Currently enabling the flag makes QP so heavy + steer_limit_constraint: false # true visualize_sampling_num: 1 - enable_manual_warm_start: true - enable_warm_start: true # false - is_fixed_point_single: false + enable_manual_warm_start: false + enable_warm_start: true + enable_optimization_validation: false common: - num_curvature_sampling_points: 5 # number of sampling points when calculating curvature - delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] + num_points: 100 # number of points for optimization [-] + delta_arc_length: 1.0 # delta arc length for optimization [m] # kinematics: # If this parameter is commented out, the parameter is set as below by default. - # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8` + # The logic could be `optimization_center_offset = vehicle_info.wheelbase * 0.8` # The 0.8 scale is adopted as it performed the best. # optimization_center_offset: 2.3 # optimization center offset from base link - # replanning & trimming trajectory param outside algorithm - replan: - max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] - max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] - max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] - - # advanced parameters to improve performance as much as possible - advanced: - eb: - common: - num_joint_buffer_points: 3 # number of joint buffer points - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. - num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points - - clearance: - clearance_for_straight_line: 0.05 # minimum optimizing range around straight points - clearance_for_joint: 0.1 # minimum optimizing range around joint points - clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing - - qp: - max_iteration: 10000 # max iteration when solving QP - eps_abs: 1.0e-8 # eps abs when solving OSQP - eps_rel: 1.0e-10 # eps rel when solving OSQP - - mpt: - bounds_search_widths: [0.45, 0.15, 0.05, 0.01] - - clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory - hard_clearance_from_road: 0.0 # clearance from road boundary[m] - soft_clearance_from_road: 0.1 # clearance from road boundary[m] - soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road + clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory + # if collision_free_constraints.option.hard_constraint is true + hard_clearance_from_road: 0.0 # clearance from road boundary[m] + # if collision_free_constraints.option.soft_constraint is true + soft_clearance_from_road: 0.1 # clearance from road boundary[m] + + # weight parameter for optimization + weight: + # collision free + soft_collision_free_weight: 1000.0 # soft weight for lateral error around the middle point + + # tracking error + lat_error_weight: 1.0 # weight for lateral error + yaw_error_weight: 0.0 # weight for yaw error + yaw_error_rate_weight: 0.0 # weight for yaw error rate + steer_input_weight: 1.0 # weight for steering input + steer_rate_weight: 1.0 # weight for steering rate + + terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point + terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point + goal_lat_error_weight: 1000.0 # weight for lateral error at path end point + goal_yaw_error_weight: 1000.0 # weight for yaw error at path end point + + # avoidance + avoidance: + max_avoidance_cost: 0.5 # [m] + avoidance_cost_margin: 0.0 # [m] + avoidance_cost_band_length: 5.0 # [m] + avoidance_cost_decrease_rate: 0.05 # decreased cost per point interval weight: - soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point - soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point - - lat_error_weight: 100.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - yaw_error_rate_weight: 0.0 # weight for yaw error rate - steer_input_weight: 10.0 # weight for steering input - steer_rate_weight: 10.0 # weight for steering rate - - obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error - obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error - obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error - near_objects_length: 30.0 # weight for yaw error - - terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point - terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point - terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point - terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point - - # check if planned trajectory is outside drivable area - collision_free_constraints: - option: - l_inf_norm: true - soft_constraint: true - hard_constraint: false - # two_step_soft_constraint: false - - vehicle_circles: - method: "rear_drive" - - uniform_circle: - num: 3 - radius_ratio: 0.8 - - fitting_uniform_circle: - num: 3 # must be greater than 1 - - rear_drive: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 - - bicycle_model: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 + lat_error_weight: 0.0 # weight for lateral error + yaw_error_weight: 10.0 # weight for yaw error + steer_input_weight: 100.0 # weight for yaw error + + # collision free constraint for optimization + collision_free_constraints: + option: + l_inf_norm: true + soft_constraint: true + hard_constraint: false + + # how to represent footprint as circles + vehicle_circles: + method: "fitting_uniform_circle" + + bicycle_model: + num_for_calculation: 3 + front_radius_ratio: 1.0 + rear_radius_ratio: 1.0 + + uniform_circle: + num: 3 + radius_ratio: 1.0 + + fitting_uniform_circle: + num: 3 + + validation: # if enable_optimization_validation is true + max_lat_error: 5.0 # [m] + max_yaw_error: 1.046 # [rad] diff --git a/planning/obstacle_avoidance_planner/debug.md b/planning/obstacle_avoidance_planner/docs/debug.md similarity index 91% rename from planning/obstacle_avoidance_planner/debug.md rename to planning/obstacle_avoidance_planner/docs/debug.md index 26d9f3b9c6954..4c8b6eb910bcc 100644 --- a/planning/obstacle_avoidance_planner/debug.md +++ b/planning/obstacle_avoidance_planner/docs/debug.md @@ -8,50 +8,50 @@ Topics for debugging will be explained in this section. - Drivable area to cover the road. Whether this area is continuous and covers the road can be checked. - `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area`, whose type is `nav_msgs/msg/OccupancyGrid` -![drivable_area](./media/drivable_area.png) +![drivable_area](../media/drivable_area.png) - **Path from behavior** - The input path of obstacle_avoidance_planner. Whether this path is continuous and the curvature is not so high can be checked. - `Path` or `PathFootprint` rviz plugin. -![behavior_path](./media/behavior_path.png) +![behavior_path](../media/behavior_path.png) - **EB trajectory** - The output trajectory of elastic band. Whether this trajectory is very smooth and a sampling width is constant can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![eb_traj](./media/eb_traj.png) +![eb_traj](../media/eb_traj.png) - **MPT reference trajectory** - The reference trajectory of model predictive trajectory. Whether this trajectory is very smooth and a sampling width is constant can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![mpt_ref_traj](./media/mpt_ref_traj.png) +![mpt_ref_traj](../media/mpt_ref_traj.png) - **MPT fixed trajectory** - The fixed trajectory around the ego of model predictive trajectory. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![mpt_fixed_traj](./media/mpt_fixed_traj.png) +![mpt_fixed_traj](../media/mpt_fixed_traj.png) - **bounds** - Lateral Distance to the road or object boundaries to check collision in model predictive trajectory. - Whether these lines' ends align the road or obstacle boundaries can be checked. - `bounds*` of `/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker` whose type is `visualization_msgs/msg/MarkerArray` -![bounds](./media/bounds.png) +![bounds](../media/bounds.png) - **MPT trajectory** - The output of model predictive trajectory. Whether this trajectory is smooth enough and inside the drivable area can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![mpt_traj](./media/mpt_traj.png) +![mpt_traj](../media/mpt_traj.png) - **Output trajectory** - The output of obstacle_avoidance_planner. Whether this trajectory is smooth enough can be checked. - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![output_traj](./media/output_traj.png) +![output_traj](../media/output_traj.png) ## Calculation cost @@ -114,4 +114,4 @@ With a script, we can plot some of above time as follows. python3 scripts/calclation_time_analyzer.py -f "solveOsqp,initOsqp,pathCallback" ``` -![calculation_cost_plot](./media/calculation_cost_plot.svg) +![calculation_cost_plot](../media/calculation_cost_plot.svg) diff --git a/planning/obstacle_avoidance_planner/docs/eb.md b/planning/obstacle_avoidance_planner/docs/eb.md new file mode 100644 index 0000000000000..6becc8b03cc24 --- /dev/null +++ b/planning/obstacle_avoidance_planner/docs/eb.md @@ -0,0 +1,120 @@ +# Elastic band + +## Abstract + +Elastic band smooths the input path. +Since the latter optimization (model predictive trajectory) is calculated on the frenet frame, path smoothing is applied here so that the latter optimization will be stable. + +Note that this smoothing process does not consider collision checking. +Therefore the output path may have a collision with road boundaries or obstacles. + +## Flowchart + +```plantuml +@startuml +title getEBTrajectory +start + +:crop trajectory; + +:insertFixedPoint; + +:resample trajectory; + +:getPaddedTrajectoryPoints + +:updateConstraint; + +:optimizeTrajectory; + +:convertOptimizedPointsToTrajectory; + +stop +@enduml +``` + +## Formulation + +We formulate a quadratic problem minimizing the diagonal length of the rhombus on each point generated by the current point and its previous and next points, shown as the red vector's length. + +![eb](../media/eb.svg){: style="width:600px"} + +Assuming that $k$'th point is $\boldsymbol{p}_k = (x_k, y_k)$, the objective function is as follows. + +$$ +\begin{align} +\ J & = \min \sum_{k=1}^{n-2} ||(\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}) - (\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1})||^2 \\ +\ & = \min \sum_{k=1}^{n-2} ||\boldsymbol{p}_{k+1} - 2 \boldsymbol{p}_{k} + \boldsymbol{p}_{k-1}||^2 \\ +\ & = \min \sum_{k=1}^{n-2} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\ +\ & = \min + \begin{pmatrix} + \ x_0 \\ + \ x_1 \\ + \ x_2 \\ + \vdots \\ + \ x_{n-3}\\ + \ x_{n-2} \\ + \ x_{n-1} \\ + \ y_0 \\ + \ y_1 \\ + \ y_2 \\ + \vdots \\ + \ y_{n-3}\\ + \ y_{n-2} \\ + \ y_{n-1} \\ + \end{pmatrix}^T + \begin{pmatrix} + 1 & -2 & 1 & 0 & \dots& \\ + -2 & 5 & -4 & 1 & 0 &\dots \\ + 1 & -4 & 6 & -4 & 1 & \\ + 0 & 1 & -4 & 6 & -4 & \\ + \vdots & 0 & \ddots&\ddots& \ddots \\ + & \vdots & & & \\ + & & & 1 & -4 & 6 & -4 & 1 \\ + & & & & 1 & -4 & 5 & -2 \\ + & & & & & 1 & -2 & 1& \\ + & & & & & & & &1 & -2 & 1 & 0 & \dots& \\ + & & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\ + & & & & & & & &1 & -4 & 6 & -4 & 1 & \\ + & & & & & & & &0 & 1 & -4 & 6 & -4 & \\ + & & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\ + & & & & & & & & & \vdots & & & \\ + & & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\ + & & & & & & & & & & & & 1 & -4 & 5 & -2 \\ + & & & & & & & & & & & & & 1 & -2 & 1& \\ + \end{pmatrix} + \begin{pmatrix} + \ x_0 \\ + \ x_1 \\ + \ x_2 \\ + \vdots \\ + \ x_{n-3}\\ + \ x_{n-2} \\ + \ x_{n-1} \\ + \ y_0 \\ + \ y_1 \\ + \ y_2 \\ + \vdots \\ + \ y_{n-3}\\ + \ y_{n-2} \\ + \ y_{n-1} \\ + \end{pmatrix} +\end{align} +$$ + +Regarding the constraint, the distance that each point can move is limited so that the path will not changed a lot but will be smoother. +In detail, the longitudinal distance that each point can move is zero, and the lateral distance is parameterized as `eb.clearance.clearance_for_fix`, `eb.clearance.clearance_for_joint` and `eb.clearance.clearance_for_smooth`. + +The following figure describes how to constrain the lateral distance to move. +The red line is where the point can move. The points for the upper and lower bound are described as $(x_k^u, y_k^u)$ and $(x_k^l, y_k^l)$, respectively. + +![eb_constraint](../media/eb_constraint.svg){: style="width:700px"} + +Based on the line equation whose slope angle is $\theta_k$ and that passes through $(x_k, y_k)$, $(x_k^u, y_k^u)$ and $(x_k^l, y_k^l)$, the lateral constraint is formulated as follows. + +$$ +C_k^l \leq C_k \leq C_k^u +$$ + +In addition, the beginning point is fixed and the end point as well if the end point is considered as the goal. +This constraint can be applied with the upper equation by changing the distance that each point can move. diff --git a/planning/obstacle_avoidance_planner/docs/mpt.md b/planning/obstacle_avoidance_planner/docs/mpt.md new file mode 100644 index 0000000000000..6c07244bdb45e --- /dev/null +++ b/planning/obstacle_avoidance_planner/docs/mpt.md @@ -0,0 +1,484 @@ +# Model predictive trajectory + +## Abstract + +Model Predictive Trajectory (MPT) calculates the trajectory that meets the following conditions. + +- Kinematically feasible for linear vehicle kinematics model +- Collision free with obstacles and road boundaries + +Conditions for collision free is considered to be not hard constraints but soft constraints. +When the optimization failed or the optimized trajectory is not collision free, the output trajectory will be previously generated trajectory. + +Trajectory near the ego must be stable, therefore the condition where trajectory points near the ego are the same as previously generated trajectory is considered, and this is the only hard constraints in MPT. + +## Flowchart + +```plantuml +@startuml +title getModelPredictiveTrajectory +start + +:calcReferencePoints; + +:calc state matrix; + +:calcValueMatrix; + +:calcObjectiveMatrix; + +:calcConstraintMatrix; + +:calcOptimizedSteerAngles; + +:calcMPTPoints; + +:publishDebugTrajectories; + +stop +@enduml +``` + +## Vehicle kinematics + +As the following figure, we consider the bicycle kinematics model in the frenet frame to track the reference path. +At time step $k$, we define lateral distance to the reference path, heading angle against the reference path, and steer angle as $y_k$, $\theta_k$, and $\delta_k$ respectively. + +![vehicle_error_kinematics](../media/vehicle_error_kinematics.png) + +Assuming that the commanded steer angle is $\delta_{des, k}$, the kinematics model in the frenet frame is formulated as follows. +We also assume that the steer angle $\delta_k$ is first-order lag to the commanded one. + +$$ +\begin{align} +y_{k+1} & = y_{k} + v \sin \theta_k dt \\ +\theta_{k+1} & = \theta_k + \frac{v \tan \delta_k}{L}dt - \kappa_k v \cos \theta_k dt \\ +\delta_{k+1} & = \delta_k - \frac{\delta_k - \delta_{des,k}}{\tau}dt +\end{align} +$$ + +### Linearization + +Then we linearize these equations. +$y_k$ and $\theta_k$ are tracking errors, so we assume that those are small enough. +Therefore $\sin \theta_k \approx \theta_k$. + +Since $\delta_k$ is a steer angle, it is not always small. +By using a reference steer angle $\delta_{\mathrm{ref}, k}$ calculated by the reference path curvature $\kappa_k$, we express $\delta_k$ with a small value $\Delta \delta_k$. + +Note that the steer angle $\delta_k$ is within the steer angle limitation $\delta_{\max}$. +When the reference steer angle $\delta_{\mathrm{ref}, k}$ is larger than the steer angle limitation $\delta_{\max}$, and $\delta_{\mathrm{ref}, k}$ is used to linearize the steer angle, $\Delta \delta_k$ is $\Delta \delta_k = \delta - \delta_{\mathrm{ref}, k} = \delta_{\max} - \delta_{\mathrm{ref}, k}$, and the absolute $\Delta \delta_k$ gets larger. +Therefore, we have to apply the steer angle limitation to $\delta_{\mathrm{ref}, k}$ as well. + +$$ +\begin{align} +\delta_{\mathrm{ref}, k} & = \mathrm{clamp}(\arctan(L \kappa_k), -\delta_{\max}, \delta_{\max}) \\ +\delta_k & = \delta_{\mathrm{ref}, k} + \Delta \delta_k, \ \Delta \delta_k \ll 1 \\ +\end{align} +$$ + +$\mathrm{clamp}(v, v_{\min}, v_{\max})$ is a function to convert $v$ to be larger than $v_{\min}$ and smaller than $v_{\max}$. + +Using this $\delta_{\mathrm{ref}, k}$, $\tan \delta_k$ is linearized as follows. + +$$ +\begin{align} +\tan \delta_k & \approx \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} \Delta \delta_k \\ +& = \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} (\delta_{\mathrm{ref}, k} - \delta_k) \\ +& = \tan \delta_{\mathrm{ref}, k} - \frac{\delta_{\mathrm{ref}, k}}{\cos^2 \delta_{\mathrm{ref}, k}} + \frac{1}{\cos^2 \delta_{\mathrm{ref}, k}} \delta_k +\end{align} +$$ + +### One-step state equation + +Based on the linearization, the error kinematics is formulated with the following linear equations, + +$$ +\begin{align} + \begin{pmatrix} + y_{k+1} \\ + \theta_{k+1} + \end{pmatrix} + = + \begin{pmatrix} + 1 & v dt \\ + 0 & 1 \\ + \end{pmatrix} + \begin{pmatrix} + y_k \\ + \theta_k \\ + \end{pmatrix} + + + \begin{pmatrix} + 0 \\ + \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ + \end{pmatrix} + \delta_{k} + + + \begin{pmatrix} + 0 \\ + \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt\\ + \end{pmatrix} +\end{align} +$$ + +which can be formulated as follows with the state $\boldsymbol{x}$, control input $u$ and some matrices, where $\boldsymbol{x} = (y_k, \theta_k)$ + +$$ +\begin{align} + \boldsymbol{x}_{k+1} = A_k \boldsymbol{x}_k + \boldsymbol{b}_k u_k + \boldsymbol{w}_k +\end{align} +$$ + +### Time-series state equation + +Then, we formulate time-series state equation by concatenating states, control inputs and matrices respectively as + +$$ +\begin{align} + \boldsymbol{x} = A \boldsymbol{x}_0 + B \boldsymbol{u} + \boldsymbol{w} +\end{align} +$$ + +where + +$$ +\begin{align} +\boldsymbol{x} = (\boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \boldsymbol{x}^T_3, \dots, \boldsymbol{x}^T_{n-1})^T \\ +\boldsymbol{u} = (u_0, u_1, u_2, \dots, u_{n-2})^T \\ +\boldsymbol{w} = (\boldsymbol{w}^T_0, \boldsymbol{w}^T_1, \boldsymbol{w}^T_2, \dots, \boldsymbol{w}^T_{n-1})^T. \\ +\end{align} +$$ + +In detail, each matrices are constructed as follows. + +$$ +\begin{align} + \begin{pmatrix} + \boldsymbol{x}_1 \\ + \boldsymbol{x}_2 \\ + \boldsymbol{x}_3 \\ + \vdots \\ + \boldsymbol{x}_{n-1} + \end{pmatrix} + = + \begin{pmatrix} + A_0 \\ + A_1 A_0 \\ + A_2 A_1 A_0\\ + \vdots \\ + \prod\limits_{k=0}^{n-1} A_{k} + \end{pmatrix} + \boldsymbol{x}_0 + + + \begin{pmatrix} + B_0 & 0 & & \dots & 0 \\ + A_0 B_0 & B_1 & 0 & \dots & 0 \\ + A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} + \end{pmatrix} + \begin{pmatrix} + u_0 \\ + u_1 \\ + u_2 \\ + \vdots \\ + u_{n-2} + \end{pmatrix} + + + \begin{pmatrix} + I & 0 & & \dots & 0 \\ + A_0 & I & 0 & \dots & 0 \\ + A_1 A_0 & A_0 & I & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{w}_0 \\ + \boldsymbol{w}_1 \\ + \boldsymbol{w}_2 \\ + \vdots \\ + \boldsymbol{w}_{n-2} + \end{pmatrix} +\end{align} +$$ + +### Free-boundary-conditioned time-series state equation + +For path planning which does not start from the current ego pose, $\boldsymbol{x}_0$ should be the design variable of optimization. +Therefore, we make $\boldsymbol{u}'$ by concatenating $\boldsymbol{x}_0$ and $\boldsymbol{u}$, and redefine $\boldsymbol{x}$ as follows. + +$$ +\begin{align} + \boldsymbol{u}' & = (\boldsymbol{x}^T_0, \boldsymbol{u}^T)^T \\ + \boldsymbol{x} & = (\boldsymbol{x}^T_0, \boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \dots, \boldsymbol{x}^T_{n-1})^T +\end{align} +$$ + +Then we get the following state equation + +$$ +\begin{align} + \boldsymbol{x}' = B \boldsymbol{u}' + \boldsymbol{w}, +\end{align} +$$ + +which is in detail + +$$ +\begin{align} + \begin{pmatrix} + \boldsymbol{x}_0 \\ + \boldsymbol{x}_1 \\ + \boldsymbol{x}_2 \\ + \boldsymbol{x}_3 \\ + \vdots \\ + \boldsymbol{x}_{n-1} + \end{pmatrix} + = + \begin{pmatrix} + I & 0 & \dots & & & 0 \\ + A_0 & B_0 & 0 & & \dots & 0 \\ + A_1 A_0 & A_0 B_0 & B_1 & 0 & \dots & 0 \\ + A_2 A_1 A_0 & A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ + \vdots & \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-1} A_k & \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{x}_0 \\ + u_0 \\ + u_1 \\ + u_2 \\ + \vdots \\ + u_{n-2} + \end{pmatrix} + + + \begin{pmatrix} + 0 & \dots & & & 0 \\ + I & 0 & & \dots & 0 \\ + A_0 & I & 0 & \dots & 0 \\ + A_1 A_0 & A_0 & I & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{w}_0 \\ + \boldsymbol{w}_1 \\ + \boldsymbol{w}_2 \\ + \vdots \\ + \boldsymbol{w}_{n-2} + \end{pmatrix}. +\end{align} +$$ + +## Objective function + +The objective function for smoothing and tracking is shown as follows, which can be formulated with value function matrices $Q, R$. + +$$ +\begin{align} +J_1 (\boldsymbol{x}', \boldsymbol{u}') & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 \\ +& = \boldsymbol{x}'^T Q \boldsymbol{x}' + \boldsymbol{u}'^T R \boldsymbol{u}' \\ +& = \boldsymbol{u}'^T H \boldsymbol{u}' + \boldsymbol{u}'^T \boldsymbol{f} +\end{align} +$$ + +As mentioned before, the constraints to be collision free with obstacles and road boundaries are formulated to be soft constraints. +Assuming that the lateral distance to the road boundaries or obstacles from the back wheel center, front wheel center, and the point between them are $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ respectively, and slack variables for each point are $\lambda_{\mathrm{base}}, \lambda_{\mathrm{top}}, \lambda_{\mathrm{mid}}$, the soft constraints can be formulated as follows. + +$$ +y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\ +y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\ +y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} \\ +0 \leq \lambda_{\mathrm{base}, k} \\ +0 \leq \lambda_{\mathrm{top}, k} \\ +0 \leq \lambda_{\mathrm{mid}, k} +$$ + +Since $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ is formulated as a linear function of $y_k$, the objective function for soft constraints is formulated as follows. + +$$ +\begin{align} +J_2 & (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol {\lambda}_\mathrm{mid})\\ +& = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k} + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k} + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k} +\end{align} +$$ + +Slack variables are also design variables for optimization. +We define a vector $\boldsymbol{v}$, that concatenates all the design variables. + +$$ +\begin{align} +\boldsymbol{v} = +\begin{pmatrix} + \boldsymbol{u}'^T & \boldsymbol{\lambda}_\mathrm{base}^T & \boldsymbol{\lambda}_\mathrm{top}^T & \boldsymbol{\lambda}_\mathrm{mid}^T +\end{pmatrix}^T +\end{align} +$$ + +The summation of these two objective functions is the objective function for the optimization problem. + +$$ +\begin{align} +\min_{\boldsymbol{v}} J (\boldsymbol{v}) = \min_{\boldsymbol{v}} J_1 (\boldsymbol{u}') + J_2 (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol{\lambda}_\mathrm{mid}) +\end{align} +$$ + +As mentioned before, we use hard constraints where some trajectory points in front of the ego are the same as the previously generated trajectory points. +This hard constraints is formulated as follows. + +$$ +\begin{align} +\delta_k = \delta_{k}^{\mathrm{prev}} (0 \leq i \leq N_{\mathrm{fix}}) +\end{align} +$$ + +Finally we transform those objective functions to the following QP problem, and solve it. + +$$ +\begin{align} +\min_{\boldsymbol{v}} \ & \frac{1}{2} \boldsymbol{v}^T \boldsymbol{H} \boldsymbol{v} + \boldsymbol{f} \boldsymbol{v} \\ +\mathrm{s.t.} \ & \boldsymbol{b}_{lower} \leq \boldsymbol{A} \boldsymbol{v} \leq \boldsymbol{b}_{upper} +\end{align} +$$ + +## Constraints + +### Steer angle limitation + +Steer angle has a limitation $\delta_{max}$ and $\delta_{min}$. +Therefore we add linear inequality equations. + +$$ +\begin{align} +\delta_{min} \leq \delta_i \leq \delta_{max} +\end{align} +$$ + +### Collision free + +To realize collision-free trajectory planning, we have to formulate constraints that the vehicle is inside the road and also does not collide with obstacles in linear equations. +For linearity, we implemented some methods to approximate the vehicle shape with a set of circles, that is reliable and easy to implement. + +- 1. Bibycle Model +- 2. Uniform Circles +- 3. Fitting Uniform Circles + +![vehicle_circles](../media/vehicle_circles.svg) + +Now we formulate the linear constraints where a set of circles on each trajectory point is collision-free. +By using the drivable area, we calculate upper and lower boundaries along reference points, which will be interpolated on any position on the trajectory. +NOTE that upper and lower boundary is left and right, respectively. + +Assuming that upper and lower boundaries are $b_l$, $b_u$ respectively, and $r$ is a radius of a circle, lateral deviation of the circle center $y'$ has to be + +$$ +b_l + r \leq y' \leq b_u - r. +$$ + +![bounds](../media/bounds.svg) + +Based on the following figure, $y'$ can be formulated as follows. + +$$ +\begin{align} +y' & = L \sin(\theta + \beta) + y \cos \beta - l \sin(\gamma - \phi_a) \\ +& = L \sin \theta \cos \beta + L \cos \theta \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \\ +& \approx L \theta \cos \beta + L \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) +\end{align} +$$ + +$$ +b_l + r - \lambda \leq y' \leq b_u - r + \lambda. +$$ + +$$ +\begin{align} +y' & = C_1 \boldsymbol{x} + C_2 \\ +& = C_1 (B \boldsymbol{v} + \boldsymbol{w}) + C_2 \\ +& = C_1 B \boldsymbol{v} + \boldsymbol{w} + C_2 +\end{align} +$$ + +Note that longitudinal position of the circle center and the trajectory point to calculate boundaries are different. +But each boundaries are vertical against the trajectory, resulting in less distortion by the longitudinal position difference since road boundaries does not change so much. +For example, if the boundaries are not vertical against the trajectory and there is a certain difference of longitudinal position between the circe center and the trajectory point, we can easily guess that there is much more distortion when comparing lateral deviation and boundaries. + +$$ +\begin{align} + A_{blk} & = + \begin{pmatrix} + C_1 B & O & \dots & O & I_{N_{ref} \times N_{ref}} & O \dots & O\\ + -C_1 B & O & \dots & O & I & O \dots & O\\ + O & O & \dots & O & I & O \dots & O + \end{pmatrix} + \in \boldsymbol{R}^{3 N_{ref} \times D_v + N_{circle} N_{ref}} \\ + \boldsymbol{b}_{lower, blk} & = + \begin{pmatrix} + \boldsymbol{b}_{lower} - C_1 \boldsymbol{w} - C_2 \\ + -\boldsymbol{b}_{upper} + C_1 \boldsymbol{w} + C_2 \\ + O + \end{pmatrix} + \in \boldsymbol{R}^{3 N_{ref}} \\ + \boldsymbol{b}_{upper, blk} & = \boldsymbol{\infty} + \in \boldsymbol{R}^{3 N_{ref}} +\end{align} +$$ + +We will explain options for optimization. + +#### L-infinity optimization + +The above formulation is called L2 norm for slack variables. +Instead, if we use L-infinity norm where slack variables are shared by enabling `l_inf_norm`. + +$$ +\begin{align} + A_{blk} = + \begin{pmatrix} + C_1 B & I_{N_{ref} \times N_{ref}} \\ + -C_1 B & I \\ + O & I + \end{pmatrix} +\in \boldsymbol{R}^{3N_{ref} \times D_v + N_{ref}} +\end{align} +$$ + +#### Two-step soft constraints + +$$ +\begin{align} +\boldsymbol{v}' = + \begin{pmatrix} + \boldsymbol{v} \\ + \boldsymbol{\lambda}^{soft_1} \\ + \boldsymbol{\lambda}^{soft_2} \\ + \end{pmatrix} + \in \boldsymbol{R}^{D_v + 2N_{slack}} +\end{align} +$$ + +$*$ depends on whether to use L2 norm or L-infinity optimization. + +$$ +\begin{align} + A_{blk} & = + \begin{pmatrix} + A^{soft_1}_{blk} \\ + A^{soft_2}_{blk} \\ + \end{pmatrix}\\ + & = + \begin{pmatrix} + C_1^{soft_1} B & & \\ + -C_1^{soft_1} B & \Huge{*} & \Huge{O} \\ + O & & \\ + C_1^{soft_2} B & & \\ + -C_1^{soft_2} B & \Huge{O} & \Huge{*} \\ + O & & + \end{pmatrix} + \in \boldsymbol{R}^{6 N_{ref} \times D_v + 2 N_{slack}} +\end{align} +$$ + +$N_{slack}$ is $N_{circle}$ when L2 optimization, or $1$ when L-infinity optimization. +$N_{circle}$ is the number of circles to check collision. diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 54aa3356909ef..95656edf5f0e4 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 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. @@ -15,246 +15,133 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ -#include "opencv2/core.hpp" -#include "opencv2/opencv.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include +#include #include #include +namespace obstacle_avoidance_planner +{ struct ReferencePoint; - struct Bounds; -using VehicleBounds = std::vector; -using SequentialBounds = std::vector; - -using BoundsCandidates = std::vector; - -struct QPParam -{ - int max_iteration; - double eps_abs; - double eps_rel; -}; -struct EBParam +struct PlannerData { - double clearance_for_fixing; - double clearance_for_straight_line; - double clearance_for_joint; - double clearance_for_only_smoothing; - QPParam qp_param; - - int num_joint_buffer_points; - int num_offset_for_begin_idx; - - double delta_arc_length_for_eb; - int num_sampling_points_for_eb; + // input + Header header; + std::vector traj_points; // converted from the input path + std::vector left_bound; + std::vector right_bound; + + // ego + geometry_msgs::msg::Pose ego_pose; + double ego_vel{}; }; -struct VehicleParam +struct TimeKeeper { - double wheelbase; - double length; - double width; - double rear_overhang; - double front_overhang; - double wheel_tread; - double right_overhang; - double left_overhang; - // double max_steer_rad; -}; - -struct ConstrainRectangle -{ - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; - double velocity; - bool is_empty_driveable_area = false; - bool is_including_only_smooth_range = true; -}; + void init() { accumulated_msg = "\n"; } -struct DebugData -{ - struct StreamWithPrint + template + TimeKeeper & operator<<(const T & msg) { - StreamWithPrint & operator<<(const std::string & s) - { - sstream << s; - if (s.back() == '\n') { - std::string tmp_str = sstream.str(); - debug_str += tmp_str; + latest_stream << msg; + return *this; + } - if (is_showing_calculation_time) { - tmp_str.pop_back(); // NOTE* remove '\n' which is unnecessary for RCLCPP_INFO_STREAM - RCLCPP_INFO_STREAM(rclcpp::get_logger("obstacle_avoidance_planner.time"), tmp_str); - } - sstream.str(""); - } - return *this; - } + void endLine() + { + const auto msg = latest_stream.str(); + accumulated_msg += msg + "\n"; + latest_stream.str(""); - StreamWithPrint & operator<<(const double d) - { - sstream << d; - return *this; + if (enable_calculation_time_info) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("obstacle_avoidance_planner.time"), msg); } + } - std::string getString() const { return debug_str; } - - bool is_showing_calculation_time; - std::string debug_str = "\n"; - std::stringstream sstream; - }; + void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - void init( - const bool local_is_showing_calculation_time, const int local_mpt_visualize_sampling_num, - const geometry_msgs::msg::Pose & local_current_ego_pose, - const std::vector & local_vehicle_circle_radiuses, - const std::vector & local_vehicle_circle_longitudinal_offsets) + void toc(const std::string & func_name, const std::string & white_spaces) { - msg_stream.is_showing_calculation_time = local_is_showing_calculation_time; - mpt_visualize_sampling_num = local_mpt_visualize_sampling_num; - current_ego_pose = local_current_ego_pose; - vehicle_circle_radiuses = local_vehicle_circle_radiuses; - vehicle_circle_longitudinal_offsets = local_vehicle_circle_longitudinal_offsets; + const double elapsed_time = stop_watch_.toc(func_name); + *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; + endLine(); } - StreamWithPrint msg_stream; - size_t mpt_visualize_sampling_num; - geometry_msgs::msg::Pose current_ego_pose; - std::vector vehicle_circle_radiuses; - std::vector vehicle_circle_longitudinal_offsets; - - boost::optional stop_pose_by_drivable_area = boost::none; - std::vector interpolated_points; - std::vector straight_points; - std::vector fixed_points; - std::vector non_fixed_points; - std::vector constrain_rectangles; - std::vector avoiding_traj_points; - std::vector avoiding_objects; - - cv::Mat clearance_map; - cv::Mat only_object_clearance_map; - cv::Mat area_with_objects_map; - - std::vector> vehicle_circles_pose; - std::vector ref_points; - - std::vector mpt_ref_poses; - std::vector lateral_errors; - std::vector yaw_errors; + std::string getLog() const { return accumulated_msg; } - std::vector eb_traj; - std::vector mpt_fixed_traj; - std::vector mpt_ref_traj; - std::vector mpt_traj; - std::vector extended_fixed_traj; - std::vector extended_non_fixed_traj; + bool enable_calculation_time_info; + std::string accumulated_msg = "\n"; + std::stringstream latest_stream; - BoundsCandidates bounds_candidates; + tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; }; -struct Trajectories +struct DebugData { - std::vector smoothed_trajectory; - std::vector mpt_ref_points; - std::vector model_predictive_trajectory; -}; + // settting + size_t mpt_visualize_sampling_num; + geometry_msgs::msg::Pose ego_pose; + std::vector vehicle_circle_radiuses; + std::vector vehicle_circle_longitudinal_offsets; -struct TrajectoryParam -{ - bool is_avoiding_unknown; - bool is_avoiding_car; - bool is_avoiding_truck; - bool is_avoiding_bus; - bool is_avoiding_bicycle; - bool is_avoiding_motorbike; - bool is_avoiding_pedestrian; - bool is_avoiding_animal; - int num_sampling_points; - double delta_arc_length_for_trajectory; - double delta_dist_threshold_for_closest_point; - double delta_yaw_threshold_for_closest_point; - double delta_yaw_threshold_for_straight; - double trajectory_length; - double forward_fixing_min_distance; - double forward_fixing_min_time; - double backward_fixing_distance; - double max_avoiding_ego_velocity_ms; - double max_avoiding_objects_velocity_ms; - double center_line_width; - double acceleration_for_non_deceleration_range; - int num_fix_points_for_extending; - double max_dist_for_extending_end_point; - double non_fixed_trajectory_length; + // mpt + std::vector ref_points; + std::vector> vehicle_circles_pose; - double ego_nearest_dist_threshold; - double ego_nearest_yaw_threshold; - bool enable_clipping_fixed_traj; + std::vector extended_traj_points; }; -struct MPTParam +struct TrajectoryParam { - bool enable_warm_start; - bool enable_manual_warm_start; - bool steer_limit_constraint; - bool fix_points_around_ego; - int num_curvature_sampling_points; - bool is_fixed_point_single; - - std::vector vehicle_circle_longitudinal_offsets; // from base_link - std::vector vehicle_circle_radiuses; - - double delta_arc_length_for_mpt_points; - - double hard_clearance_from_road; - double soft_clearance_from_road; - double soft_second_clearance_from_road; - double extra_desired_clearance_from_road; - double clearance_from_object; - double soft_avoidance_weight; - double soft_second_avoidance_weight; - - double lat_error_weight; - double yaw_error_weight; - double yaw_error_rate_weight; - - double near_objects_length; + TrajectoryParam() = default; + explicit TrajectoryParam(rclcpp::Node * node) + { + output_backward_traj_length = + node->declare_parameter("common.output_backward_traj_length"); + output_delta_arc_length = node->declare_parameter("common.output_delta_arc_length"); + } - double terminal_lat_error_weight; - double terminal_yaw_error_weight; - double terminal_path_lat_error_weight; - double terminal_path_yaw_error_weight; + void onParam(const std::vector & parameters) + { + using tier4_autoware_utils::updateParam; - double steer_input_weight; - double steer_rate_weight; + // common + updateParam( + parameters, "common.output_backward_traj_length", output_backward_traj_length); + updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); + } - double obstacle_avoid_lat_error_weight; - double obstacle_avoid_yaw_error_weight; - double obstacle_avoid_steer_input_weight; + double output_delta_arc_length; + double output_backward_traj_length; +}; - double optimization_center_offset; - double max_steer_rad; +struct EgoNearestParam +{ + EgoNearestParam() = default; + explicit EgoNearestParam(rclcpp::Node * node) + { + dist_threshold = node->declare_parameter("ego_nearest_dist_threshold"); + yaw_threshold = node->declare_parameter("ego_nearest_yaw_threshold"); + } - std::vector bounds_search_widths; + void onParam(const std::vector & parameters) + { + using tier4_autoware_utils::updateParam; + updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); + updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); + } - bool soft_constraint; - bool hard_constraint; - bool l_inf_norm; - bool two_step_soft_constraint; - bool plan_from_ego; - double max_plan_from_ego_length; + double dist_threshold{0.0}; + double yaw_threshold{0.0}; }; +} // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp new file mode 100644 index 0000000000000..8753699ab9d4b --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 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 OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "rclcpp/clock.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include +#include + +namespace obstacle_avoidance_planner +{ +MarkerArray getDebugMarker( + const DebugData & debug_data, + const std::vector & optimized_points, + const vehicle_info_util::VehicleInfo & vehicle_info); +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp deleted file mode 100644 index ae412c046f98c..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2020 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 OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ - -#include "eigen3/Eigen/Core" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include - -class EBPathOptimizer -{ -private: - struct CandidatePoints - { - std::vector fixed_points; - std::vector non_fixed_points; - int begin_path_idx; - int end_path_idx; - }; - - struct Anchor - { - geometry_msgs::msg::Pose pose; - }; - - struct ConstrainRectangles - { - ConstrainRectangle object_constrain_rectangle; - ConstrainRectangle road_constrain_rectangle; - }; - - struct ConstrainLines - { - double x_coef; - double y_coef; - double lower_bound; - double upper_bound; - }; - - struct Constrain - { - ConstrainLines top_and_bottom; - ConstrainLines left_and_right; - }; - - struct Rectangle - { - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; - }; - - struct OccupancyMaps - { - std::vector> object_occupancy_map; - std::vector> road_occupancy_map; - }; - - const bool is_showing_debug_info_; - const double epsilon_; - - const QPParam qp_param_; - const TrajectoryParam traj_param_; - const EBParam eb_param_; - const VehicleParam vehicle_param_; - - Eigen::MatrixXd default_a_matrix_; - std::unique_ptr osqp_solver_ptr_; - - double current_ego_vel_; - - mutable tier4_autoware_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; - - Eigen::MatrixXd makePMatrix(); - - Eigen::MatrixXd makeAMatrix(); - - Anchor getAnchor( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points) const; - - Constrain getConstrainFromConstrainRectangle( - const geometry_msgs::msg::Point & interpolated_point, - const ConstrainRectangle & constrain_range); - - ConstrainLines getConstrainLines( - const double dx, const double dy, const geometry_msgs::msg::Point & point, - const geometry_msgs::msg::Point & opposite_point); - - ConstrainRectangle getConstrainRectangle(const Anchor & anchor, const double clearance) const; - - ConstrainRectangle getConstrainRectangle( - const Anchor & anchor, const double min_x, const double max_x, const double min_y, - const double max_y) const; - - int getStraightLineIdx( - const std::vector & interpolated_points, - const int farthest_point_idx, - std::vector & debug_detected_straight_points); - - boost::optional> getConstrainRectangleVec( - const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx); - - std::vector getPaddedInterpolatedPoints( - const std::vector & interpolated_points, const int farthest_idx); - - int getNumFixedPoints( - const std::vector & fixed_points, - const std::vector & interpolated_points, const int farthest_idx); - - boost::optional> - getOptimizedTrajectory( - const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, - DebugData & debug_data); - - void updateConstrain( - const std::vector & interpolated_points, - const std::vector & rectangle_points); - - std::vector convertOptimizedPointsToTrajectory( - const std::vector optimized_points, const std::vector & constraint, - const int farthest_idx); - - std::vector getFixedPoints( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_optimized_points); - - CandidatePoints getCandidatePoints( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_trajs, DebugData & debug_data); - - CandidatePoints getDefaultCandidatePoints( - const std::vector & path_points); - - std::pair, int64_t> solveQP(); - - boost::optional> - calculateTrajectory( - const std::vector & padded_interpolated_points, - const std::vector & constrain_rectangles, const int farthest_idx, - DebugData & debug_data); - -public: - EBPathOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, const EBParam & eb_param, - const VehicleParam & vehicle_param); - - boost::optional> getEBTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, const double current_ego_vel, - DebugData & debug_data); -}; - -#endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp new file mode 100644 index 0000000000000..e4ac55dba5f4e --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_smoother.hpp @@ -0,0 +1,132 @@ +// Copyright 2023 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 OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ + +#include "eigen3/Eigen/Core" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "osqp_interface/osqp_interface.hpp" + +#include +#include +#include +#include +#include + +namespace obstacle_avoidance_planner +{ +class EBPathSmoother +{ +public: + EBPathSmoother( + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const TrajectoryParam & traj_param, const std::shared_ptr time_keeper_ptr); + + std::optional> getEBTrajectory( + const PlannerData & planner_data); + + void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); + void resetPreviousData(); + void onParam(const std::vector & parameters); + +private: + struct EBParam + { + // qp + struct QPParam + { + int max_iteration; + double eps_abs; + double eps_rel; + }; + + EBParam() = default; + explicit EBParam(rclcpp::Node * node); + void onParam(const std::vector & parameters); + + // option + bool enable_warm_start; + bool enable_optimization_validation; + + // common + double delta_arc_length; + int num_points; + + // clearance + int num_joint_points; + double clearance_for_fix; + double clearance_for_joint; + double clearance_for_smooth; + + // weight + double smooth_weight; + double lat_error_weight; + + // qp + QPParam qp_param; + + // validation + double max_validation_error; + }; + + struct Constraint2d + { + struct Constraint + { + Eigen::Vector2d coef; + double upper_bound; + double lower_bound; + }; + + Constraint lon; + Constraint lat; + }; + + // arguments + bool enable_debug_info_; + EgoNearestParam ego_nearest_param_; + TrajectoryParam traj_param_; + EBParam eb_param_; + mutable std::shared_ptr time_keeper_ptr_; + rclcpp::Logger logger_; + + // publisher + rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; + rclcpp::Publisher::SharedPtr debug_eb_fixed_traj_pub_; + + std::unique_ptr osqp_solver_ptr_; + std::shared_ptr> prev_eb_traj_points_ptr_{nullptr}; + + std::vector insertFixedPoint( + const std::vector & traj_point) const; + + std::tuple, size_t> getPaddedTrajectoryPoints( + const std::vector & traj_points) const; + + void updateConstraint( + const std_msgs::msg::Header & header, const std::vector & traj_points, + const bool is_goal_contained, const int pad_start_idx); + + std::optional> optimizeTrajectory(); + + std::optional> + convertOptimizedPointsToTrajectory( + const std::vector & optimized_points, const std::vector & traj_points, + const int pad_start_idx) const; +}; +} // namespace obstacle_avoidance_planner + +#endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_SMOOTHER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index d48f1f384ae52..4c70a02ce650b 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -11,83 +11,35 @@ // 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. -/* - * This Code is inspired by code from https://github.com/LiJiangnanBit/path_optimizer - * - * MIT License - * - * Copyright (c) 2020 Li Jiangnan - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ #ifndef OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Sparse" +#include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "obstacle_avoidance_planner/state_equation_generator.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/optional.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include +#include +#include +#include #include -enum class CollisionType { NO_COLLISION = 0, OUT_OF_SIGHT = 1, OUT_OF_ROAD = 2, OBJECT = 3 }; - +namespace obstacle_avoidance_planner +{ struct Bounds { - Bounds() = default; - Bounds( - const double lower_bound_, const double upper_bound_, CollisionType lower_collision_type_, - CollisionType upper_collision_type_) - : lower_bound(lower_bound_), - upper_bound(upper_bound_), - lower_collision_type(lower_collision_type_), - upper_collision_type(upper_collision_type_) - { - } - double lower_bound; double upper_bound; - CollisionType lower_collision_type; - CollisionType upper_collision_type; - - bool hasCollisionWithRightObject() const { return lower_collision_type == CollisionType::OBJECT; } - - bool hasCollisionWithLeftObject() const { return upper_collision_type == CollisionType::OBJECT; } - - bool hasCollisionWithObject() const - { - return hasCollisionWithRightObject() || hasCollisionWithLeftObject(); - } - static Bounds lerp(Bounds prev_bounds, Bounds next_bounds, double ratio) { const double lower_bound = @@ -95,14 +47,7 @@ struct Bounds const double upper_bound = interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); - if (ratio < 0.5) { - return Bounds{ - lower_bound, upper_bound, prev_bounds.lower_collision_type, - prev_bounds.upper_collision_type}; - } - - return Bounds{ - lower_bound, upper_bound, next_bounds.lower_collision_type, next_bounds.upper_collision_type}; + return Bounds{lower_bound, upper_bound}; } void translate(const double offset) @@ -112,53 +57,72 @@ struct Bounds } }; +struct KinematicState +{ + double lat{0.0}; + double yaw{0.0}; + + Eigen::Vector2d toEigenVector() const { return Eigen::Vector2d{lat, yaw}; } +}; + struct ReferencePoint { - geometry_msgs::msg::Point p; - double k = 0; - double v = 0; - double yaw = 0; - double s = 0; - double alpha = 0.0; - Bounds bounds; - bool near_objects; - - // NOTE: fix_kinematic_state is used for two purposes - // one is fixing points around ego for stability - // second is fixing current ego pose when no velocity for planning from ego pose - boost::optional fix_kinematic_state = boost::none; - bool plan_from_ego = true; - Eigen::Vector2d optimized_kinematic_state; - double optimized_input; - - // - std::vector> beta; - VehicleBounds vehicle_bounds; - - // SequentialBoundsCandidates sequential_bounds_candidates; - std::vector vehicle_bounds_poses; // for debug visualization + geometry_msgs::msg::Pose pose; + double longitudinal_velocity_mps; + + // additional information + double curvature{0.0}; + double delta_arc_length{0.0}; + double alpha{0.0}; // for minimizing lateral error + Bounds bounds{}; // bounds on `pose` + std::vector> beta{}; // for collision-free constraint + double normalized_avoidance_cost{0.0}; + + // bounds and its local pose on each collision-free constraint + std::vector bounds_on_constraints{}; + std::vector pose_on_constraints{}; + + // optimization result + std::optional fixed_kinematic_state{std::nullopt}; + KinematicState optimized_kinematic_state{}; + double optimized_input{}; + std::optional> slack_variables{std::nullopt}; + + double getYaw() const { return tf2::getYaw(pose.orientation); } + + geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const + { + auto pose_with_deviation = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); + pose_with_deviation.orientation = + tier4_autoware_utils::createQuaternionFromYaw(getYaw() + yaw_dev); + return pose_with_deviation; + } }; class MPTOptimizer { -private: - struct MPTMatrix - { - // Eigen::MatrixXd Aex; - Eigen::MatrixXd Bex; - Eigen::VectorXd Wex; - // Eigen::SparseMatrix Cex; - // Eigen::SparseMatrix Qex; - // Eigen::SparseMatrix Rex; - // Eigen::MatrixXd R1ex; - // Eigen::MatrixXd R2ex; - // Eigen::MatrixXd Uref_ex; - }; +public: + MPTOptimizer( + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, + const std::shared_ptr debug_data_ptr, + const std::shared_ptr time_keeper_ptr); + + std::optional> getModelPredictiveTrajectory( + const PlannerData & planner_data, const std::vector & smoothed_points); + + void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); + void resetPreviousData(); + void onParam(const std::vector & parameters); + double getTrajectoryLength() const; + int getNumberOfPoints() const; + +private: struct ValueMatrix { - Eigen::SparseMatrix Qex; - Eigen::SparseMatrix Rex; + Eigen::SparseMatrix Q; + Eigen::SparseMatrix R; }; struct ObjectiveMatrix @@ -174,123 +138,161 @@ class MPTOptimizer Eigen::VectorXd upper_bound; }; - struct MPTTrajs + struct MPTParam { - std::vector mpt; - std::vector ref_points; + explicit MPTParam(rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info); + MPTParam() = default; + void onParam(const std::vector & parameters); + + // option + bool enable_warm_start; + bool enable_manual_warm_start; + bool enable_optimization_validation; + bool steer_limit_constraint; + int mpt_visualize_sampling_num; // for debug + + // common + double delta_arc_length; + int num_points; + + // kinematics + double optimization_center_offset; + double max_steer_rad; + + // clearance + double hard_clearance_from_road; + double soft_clearance_from_road; + double soft_collision_free_weight; + + // weight + double lat_error_weight; + double yaw_error_weight; + double yaw_error_rate_weight; + + double terminal_lat_error_weight; + double terminal_yaw_error_weight; + double goal_lat_error_weight; + double goal_yaw_error_weight; + + double steer_input_weight; + double steer_rate_weight; + + // avoidance + double max_avoidance_cost; + double avoidance_cost_margin; + double avoidance_cost_band_length; + double avoidance_cost_decrease_rate; + double avoidance_lat_error_weight; + double avoidance_yaw_error_weight; + double avoidance_steer_input_weight; + + // constraint type + bool soft_constraint; + bool hard_constraint; + bool l_inf_norm; + + // vehicle circles + std::string vehicle_circles_method; + int vehicle_circles_uniform_circle_num; + double vehicle_circles_uniform_circle_radius_ratio; + int vehicle_circles_bicycle_model_num; + double vehicle_circles_bicycle_model_rear_radius_ratio; + double vehicle_circles_bicycle_model_front_radius_ratio; + int vehicle_circles_fitting_uniform_circle_num; + + // validation + double max_validation_lat_error; + double max_validation_yaw_error; }; - const double osqp_epsilon_ = 1.0e-3; + // publisher + rclcpp::Publisher::SharedPtr debug_fixed_traj_pub_; + rclcpp::Publisher::SharedPtr debug_ref_traj_pub_; + rclcpp::Publisher::SharedPtr debug_mpt_traj_pub_; - bool is_showing_debug_info_; + // argument + bool enable_debug_info_; + EgoNearestParam ego_nearest_param_; + vehicle_info_util::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; - VehicleParam vehicle_param_; + mutable std::shared_ptr debug_data_ptr_; + mutable std::shared_ptr time_keeper_ptr_; + rclcpp::Logger logger_; MPTParam mpt_param_; - std::unique_ptr vehicle_model_ptr_; - std::unique_ptr osqp_solver_ptr_; - geometry_msgs::msg::Pose current_ego_pose_; - double current_ego_vel_; - - int prev_mat_n = 0; - int prev_mat_m = 0; - - mutable tier4_autoware_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; + StateEquationGenerator state_equation_generator_; + std::unique_ptr osqp_solver_ptr_; - std::vector getReferencePoints( - const std::vector & points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_mpt_points, DebugData & debug_data) const; + const double osqp_epsilon_ = 1.0e-3; - void calcPlanningFromEgo(std::vector & ref_points) const; + // vehicle circles + std::vector vehicle_circle_longitudinal_offsets_; // from base_link + std::vector vehicle_circle_radiuses_; - /* - std::vector convertToReferencePoints( - const std::vector & points, - const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, - const CVMaps & maps, DebugData & debug_data) const; - */ + // previous data + int prev_mat_n_ = 0; + int prev_mat_m_ = 0; + std::shared_ptr> prev_ref_points_ptr_{nullptr}; - std::vector getFixedReferencePoints( - const std::vector & points, - const std::unique_ptr & prev_trajs) const; + void updateVehicleCircles(); - void calcBounds( + std::vector calcReferencePoints( + const PlannerData & planner_data, const std::vector & smoothed_points) const; + void updateCurvature( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const; + void updateOrientation( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const; + void updateBounds( std::vector & ref_points, const std::vector & left_bound, - const std::vector & right_bound, DebugData & debug_data) const; - - void calcVehicleBounds(std::vector & ref_points, DebugData & debug_data) const; - - // void calcFixState( - // std::vector & ref_points, - // const std::unique_ptr & prev_trajs) const; - - void calcOrientation(std::vector & ref_points) const; - - void calcCurvature(std::vector & ref_points) const; - - void calcArcLength(std::vector & ref_points) const; - - void calcExtraPoints( + const std::vector & right_bound) const; + void updateVehicleBounds( std::vector & ref_points, - const std::unique_ptr & prev_trajs) const; - - /* - * predict equation: Xec = Aex * x0 + Bex * Uex + Wex - * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * - * Uex Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) - */ - MPTMatrix generateMPTMatrix( - const std::vector & reference_points, DebugData & debug_data) const; + const SplineInterpolationPoints2d & ref_points_spline) const; + void updateFixedPoint(std::vector & ref_points) const; + void updateDeltaArcLength(std::vector & ref_points) const; + void updateExtraPoints(std::vector & ref_points) const; - ValueMatrix generateValueMatrix( + ValueMatrix calcValueMatrix( const std::vector & reference_points, - const std::vector & path_points, - DebugData & debug_data) const; + const std::vector & traj_points) const; - void addSteerWeightR( - std::vector> & Rex_triplet_vec, + ObjectiveMatrix calcObjectiveMatrix( + const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & obj_mat, const std::vector & ref_points) const; - boost::optional executeOptimization( - const std::unique_ptr & prev_trajs, const bool enable_avoidance, - const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, - const std::vector & ref_points, DebugData & debug_data); + ConstraintMatrix calcConstraintMatrix( + const StateEquationGenerator::Matrix & mpt_mat, + const std::vector & ref_points) const; - std::vector getMPTPoints( - std::vector & fixed_ref_points, - std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_matrix, DebugData & debug_data); + std::optional calcOptimizedSteerAngles( + const std::vector & ref_points, const ObjectiveMatrix & obj_mat, + const ConstraintMatrix & const_mat); + Eigen::VectorXd calcInitialSolutionForManualWarmStart( + const std::vector & ref_points, + const std::vector & prev_ref_points) const; + std::pair updateMatrixForManualWarmStart( + const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat, + const std::optional & u0) const; - std::vector getMPTFixedPoints( + void addSteerWeightR( + std::vector> & R_triplet_vec, const std::vector & ref_points) const; - ObjectiveMatrix getObjectiveMatrix( - const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, - [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const; + std::optional> calcMPTPoints( + std::vector & ref_points, const Eigen::VectorXd & U, + const StateEquationGenerator::Matrix & mpt_matrix) const; - ConstraintMatrix getConstraintMatrix( - const bool enable_avoidance, const MPTMatrix & mpt_mat, - const std::vector & ref_points, DebugData & debug_data) const; - -public: - MPTOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, - const VehicleParam & vehicle_param, const MPTParam & mpt_param); + void publishDebugTrajectories( + const std_msgs::msg::Header & header, const std::vector & ref_points, + const std::vector & mpt_traj_points) const; + std::vector extractFixedPoints( + const std::vector & ref_points) const; - boost::optional getModelPredictiveTrajectory( - const bool enable_avoidance, - const std::vector & smoothed_points, - const std::vector & path_points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_trajs, - const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, - DebugData & debug_data); + size_t getNumberOfSlackVariables() const; + std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; - +} // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 0f843926e9be2..b51b35fa99d49 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -11,305 +11,126 @@ // 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 OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" +#include "motion_utils/motion_utils.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "opencv2/core.hpp" -#include "rclcpp/clock.hpp" +#include "obstacle_avoidance_planner/replan_checker.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" -#include "tier4_planning_msgs/msg/enable_avoidance.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include "boost/optional.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include #include +#include #include #include -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using nav_msgs::msg::OccupancyGrid; -using nav_msgs::msg::Odometry; - -namespace +namespace obstacle_avoidance_planner { -template -boost::optional lerpPose( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) -{ - constexpr double epsilon = 1e-6; - - const double closest_to_target_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); - const double seg_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); - - const auto & closest_pose = points[closest_seg_idx].pose; - const auto & next_pose = points[closest_seg_idx + 1].pose; - - geometry_msgs::msg::Pose interpolated_pose; - if (std::abs(seg_dist) < epsilon) { - interpolated_pose.position.x = next_pose.position.x; - interpolated_pose.position.y = next_pose.position.y; - interpolated_pose.position.z = next_pose.position.z; - interpolated_pose.orientation = next_pose.orientation; - } else { - const double ratio = closest_to_target_dist / seg_dist; - if (ratio < 0 || 1 < ratio) { - return {}; - } - - interpolated_pose.position.x = - interpolation::lerp(closest_pose.position.x, next_pose.position.x, ratio); - interpolated_pose.position.y = - interpolation::lerp(closest_pose.position.y, next_pose.position.y, ratio); - interpolated_pose.position.z = - interpolation::lerp(closest_pose.position.z, next_pose.position.z, ratio); - - const double closest_yaw = tf2::getYaw(closest_pose.orientation); - const double next_yaw = tf2::getYaw(next_pose.orientation); - const double interpolated_yaw = interpolation::lerp(closest_yaw, next_yaw, ratio); - interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(interpolated_yaw); - } - return interpolated_pose; -} - -template -double lerpTwistX( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) -{ - if (points.size() == 1) { - return points.at(0).longitudinal_velocity_mps; - } - - constexpr double epsilon = 1e-6; - - const double closest_to_target_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); - const double seg_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); - - const double closest_vel = points[closest_seg_idx].longitudinal_velocity_mps; - const double next_vel = points[closest_seg_idx + 1].longitudinal_velocity_mps; - - if (std::abs(seg_dist) < epsilon) { - return next_vel; - } - - const double ratio = std::min(1.0, std::max(0.0, closest_to_target_dist / seg_dist)); - return interpolation::lerp(closest_vel, next_vel, ratio); -} - -template -double lerpPoseZ( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) -{ - if (points.size() == 1) { - return points.at(0).pose.position.z; - } - - constexpr double epsilon = 1e-6; - - const double closest_to_target_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); - const double seg_dist = - motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); - - const double closest_z = points[closest_seg_idx].pose.position.z; - const double next_z = points[closest_seg_idx + 1].pose.position.z; - - return std::abs(seg_dist) < epsilon - ? next_z - : interpolation::lerp(closest_z, next_z, closest_to_target_dist / seg_dist); -} -} // namespace - class ObstacleAvoidancePlanner : public rclcpp::Node { public: - struct PlannerData - { - Path path; - geometry_msgs::msg::Pose ego_pose; - double ego_vel; - std::vector objects; - }; - explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); -private: - rclcpp::Clock logger_ros_clock_; - int eb_solved_count_; - bool is_driving_forward_{true}; - - bool is_publishing_debug_visualization_marker_; - bool is_publishing_area_with_objects_; - bool is_publishing_object_clearance_map_; - bool is_publishing_clearance_map_; - bool is_showing_debug_info_; - bool is_showing_calculation_time_; - bool is_stopping_if_outside_drivable_area_; - bool enable_avoidance_; - bool enable_pre_smoothing_; - bool skip_optimization_; - bool reset_prev_optimization_; - bool is_considering_footprint_edges_; - - // vehicle stop margin - double vehicle_stop_margin_outside_drivable_area_; - - // vehicle circles info for for mpt constraints - std::string vehicle_circle_method_; - int vehicle_circle_num_for_calculation_; - std::vector vehicle_circle_radius_ratios_; - - // params for replan - double max_path_shape_change_dist_for_replan_; - double max_ego_moving_dist_for_replan_; - double max_delta_time_sec_for_replan_; - - // core algorithm - std::unique_ptr eb_path_optimizer_ptr_; - std::unique_ptr mpt_optimizer_ptr_; - - // params - TrajectoryParam traj_param_; - EBParam eb_param_; - VehicleParam vehicle_param_; - MPTParam mpt_param_; - int mpt_visualize_sampling_num_; +protected: // for the static_centerline_optimizer package + // TODO(murooka) move this node to common + class DrivingDirectionChecker + { + public: + bool isDrivingForward(const std::vector & path_points) + { + const auto is_driving_forward = motion_utils::isDrivingForward(path_points); + is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; + return is_driving_forward_; + } - // variables for debug - mutable DebugData debug_data_; - mutable tier4_autoware_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; + private: + bool is_driving_forward_{true}; + }; + DrivingDirectionChecker driving_direction_checker_{}; + + // argument variables + vehicle_info_util::VehicleInfo vehicle_info_{}; + mutable std::shared_ptr debug_data_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_ptr_{nullptr}; + + // flags for some functions + bool enable_pub_debug_marker_; + bool enable_debug_info_; + bool enable_outside_drivable_area_stop_; + bool enable_smoothing_; + bool enable_skip_optimization_; + bool enable_reset_prev_optimization_; + bool use_footprint_polygon_for_outside_drivable_area_check_; + + // core algorithms + std::shared_ptr replan_checker_ptr_{nullptr}; + std::shared_ptr eb_path_smoother_ptr_{nullptr}; + std::shared_ptr mpt_optimizer_ptr_{nullptr}; + + // parameters + TrajectoryParam traj_param_{}; + EgoNearestParam ego_nearest_param_{}; // variables for subscribers - std::unique_ptr current_odometry_ptr_; - std::unique_ptr objects_ptr_; + Odometry::SharedPtr ego_state_ptr_; // variables for previous information - std::unique_ptr prev_ego_pose_ptr_; - std::unique_ptr prev_optimal_trajs_ptr_; - std::unique_ptr> prev_path_points_ptr_; - std::unique_ptr prev_replanned_time_ptr_; + std::shared_ptr> prev_optimized_traj_points_ptr_; - // ROS + // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; - rclcpp::Publisher::SharedPtr debug_extended_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr debug_extended_non_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; - rclcpp::Publisher::SharedPtr debug_mpt_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr debug_mpt_ref_traj_pub_; - rclcpp::Publisher::SharedPtr debug_mpt_traj_pub_; - - rclcpp::Publisher::SharedPtr debug_markers_pub_; - rclcpp::Publisher::SharedPtr debug_wall_markers_pub_; - rclcpp::Publisher::SharedPtr debug_clearance_map_pub_; - rclcpp::Publisher::SharedPtr debug_object_clearance_map_pub_; - rclcpp::Publisher::SharedPtr debug_area_with_objects_pub_; - rclcpp::Publisher::SharedPtr debug_msg_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr is_avoidance_sub_; - // callback function for dynamic parameters + // debug publisher + rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; + rclcpp::Publisher::SharedPtr debug_markers_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + + // parameter callback rcl_interfaces::msg::SetParametersResult onParam( const std::vector & parameters); OnSetParametersCallbackHandle::SharedPtr set_param_res_; - // subscriber callback functions - void onOdometry(const Odometry::SharedPtr); - void onObjects(const PredictedObjects::SharedPtr); - void onEnableAvoidance(const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr); + // subscriber callback function void onPath(const Path::SharedPtr); - // functions - void resetPlanning(); - void resetPrevOptimization(); + // reset functions + void initializePlanning(); + void resetPreviousData(); + // main functions + bool isDataReady(const Path & path, rclcpp::Clock clock) const; + PlannerData createPlannerData(const Path & path) const; std::vector generateOptimizedTrajectory(const PlannerData & planner_data); - - // functions for replan - bool checkReplan(const PlannerData & planner_data); - bool isPathShapeChanged(const PlannerData & planner_data); - bool isPathGoalChanged(const PlannerData & planner_data); - bool isEgoNearToPrevTrajectory(const geometry_msgs::msg::Pose & ego_pose); - - Trajectory generateTrajectory(const PlannerData & planner_data); - - Trajectories optimizeTrajectory(const PlannerData & planner_data); - - Trajectories getPrevTrajs(const std::vector & path_points) const; - - void calcVelocity( - const std::vector & path_points, std::vector & traj_points) const; - - void insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points); - - void publishDebugDataInOptimization( - const PlannerData & planner_data, const std::vector & traj_points); - - Trajectories makePrevTrajectories( - const std::vector & path_points, const Trajectories & trajs, - const PlannerData & planner_data); - - std::vector generatePostProcessedTrajectory( - const std::vector & path_points, - const std::vector & merged_optimized_points, const PlannerData & planner_data); - - std::vector getExtendedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points); - - std::vector generateFineTrajectoryPoints( - const std::vector & path_points, - const std::vector & traj_points) const; - - std::vector alignVelocity( - const std::vector & fine_traj_points, - const std::vector & path_points, + std::vector extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_points) const; + void publishDebugData(const Header & header) const; + + // functions in generateOptimizedTrajectory + std::vector optimizeTrajectory(const PlannerData & planner_data); + std::vector getPrevOptimizedTrajectory( const std::vector & traj_points) const; - - void publishDebugDataInMain(const Path & path) const; - - template - size_t findEgoNearestIndex( - const std::vector & points, const geometry_msgs::msg::Pose & ego_pose) - { - return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, ego_pose, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold); - } + void applyInputVelocity( + std::vector & output_traj_points, + const std::vector & input_traj_points, + const geometry_msgs::msg::Pose & ego_pose) const; + void insertZeroVelocityOutsideDrivableArea( + const PlannerData & planner_data, std::vector & traj_points) const; + void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; + void publishDebugMarkerOfOptimization(const std::vector & traj_points) const; }; +} // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp new file mode 100644 index 0000000000000..6b456a6757626 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp @@ -0,0 +1,63 @@ +// Copyright 2023 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 OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" + +#include + +#include +#include + +namespace obstacle_avoidance_planner +{ +class ReplanChecker +{ +public: + explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param); + void onParam(const std::vector & parameters); + + bool isResetRequired(const PlannerData & planner_data); + + bool isReplanRequired(const rclcpp::Time & current_time); + +private: + EgoNearestParam ego_nearest_param_; + rclcpp::Logger logger_; + + // previous variables for isResetRequired + std::shared_ptr> prev_traj_points_ptr_{nullptr}; + std::shared_ptr prev_ego_pose_ptr_{nullptr}; + + // previous variable for isReplanRequired + std::shared_ptr prev_replanned_time_ptr_{nullptr}; + + // algorithm parameters + double max_path_shape_around_ego_lat_dist_; + double max_ego_moving_dist_; + double max_goal_moving_dist_; + double max_delta_time_sec_; + + bool isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; +}; +} // namespace obstacle_avoidance_planner + +#endif // OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp new file mode 100644 index 0000000000000..d4f70e8e09227 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp @@ -0,0 +1,61 @@ +// Copyright 2023 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 OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" + +#include +#include + +namespace obstacle_avoidance_planner +{ +struct ReferencePoint; + +class StateEquationGenerator +{ +public: + struct Matrix + { + Eigen::MatrixXd B; + Eigen::VectorXd W; + }; + + StateEquationGenerator() = default; + StateEquationGenerator( + const double wheel_base, const double max_steer_rad, + const std::shared_ptr time_keeper_ptr) + : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), + time_keeper_ptr_(time_keeper_ptr) + { + } + + int getDimX() const { return vehicle_model_ptr_->getDimX(); } + int getDimU() const { return vehicle_model_ptr_->getDimU(); } + + // time-series state equation: x = B u + W (u includes x_0) + // NOTE: one-step state equation is x_t+1 = Ad x_t + Bd u + Wd. + Matrix calcMatrix(const std::vector & ref_points) const; + + Eigen::VectorXd predict(const Matrix & mat, const Eigen::VectorXd U) const; + +private: + std::unique_ptr vehicle_model_ptr_; + mutable std::shared_ptr time_keeper_ptr_; +}; +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp new file mode 100644 index 0000000000000..0f45e1223551f --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 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 OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ + +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/header.hpp" +#include "tier4_debug_msgs/msg/string_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace obstacle_avoidance_planner +{ +// std_msgs +using std_msgs::msg::Header; +// planning +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +// navigation +using nav_msgs::msg::Odometry; +// visualization +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +// debug +using tier4_debug_msgs::msg::StringStamped; +} // namespace obstacle_avoidance_planner + +#endif // OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp deleted file mode 100644 index 506587f63f5e6..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2020 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 OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ - -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "opencv2/core.hpp" -#include "rclcpp/clock.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include -#include -#include - -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; - -namespace debug_utils -{ -visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - DebugData & debug_data, - const std::vector & optimized_points, - const VehicleParam & vehicle_param, const bool is_showing_debug_detail); - -visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( - DebugData & debug_data, const VehicleParam & vehicle_param); -} // namespace debug_utils - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp new file mode 100644 index 0000000000000..6673058ba59a2 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2023 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 OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); + +template <> +geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); +} // namespace tier4_autoware_utils + +namespace obstacle_avoidance_planner +{ +namespace geometry_utils +{ +template +bool isSamePoint(const T1 & t1, const T2 & t2) +{ + const auto p1 = tier4_autoware_utils::getPoint(t1); + const auto p2 = tier4_autoware_utils::getPoint(t2); + + constexpr double epsilon = 1e-6; + if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { + return false; + } + return true; +} + +bool isOutsideDrivableAreaFromRectangleFootprint( + const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, + const std::vector & right_bound, + const vehicle_info_util::VehicleInfo & vehicle_info, + const bool use_footprint_polygon_for_outside_drivable_area_check); +} // namespace geometry_utils +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp new file mode 100644 index 0000000000000..ba72dadc35608 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -0,0 +1,284 @@ +// Copyright 2023 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 OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); + +template <> +geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); + +template <> +double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p); +} // namespace tier4_autoware_utils + +namespace obstacle_avoidance_planner +{ +namespace trajectory_utils +{ +template +std::optional getPointIndexAfter( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double offset) +{ + if (points.empty()) { + return std::nullopt; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + + // search forward + if (sum_length < offset) { + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (offset < sum_length) { + return i - 1; + } + } + + return std::nullopt; + } + + // search backward + for (size_t i = target_seg_idx; 0 < i; + --i) { // NOTE: use size_t since i is always positive value + sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (sum_length < offset) { + return i - 1; + } + } + + return 0; +} + +template +T cropForwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length) { + const size_t end_idx = i; + return T{points.begin(), points.begin() + end_idx}; + } + } + + return points; +} + +template +T cropBackwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (size_t i = target_seg_idx; 0 < i; + --i) { // NOTE: use size_t since i is always positive value + sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (sum_length < backward_length) { + const size_t begin_idx = i - 1; + return T{points.begin() + begin_idx, points.end()}; + } + } + + return points; +} + +template +T cropPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + // NOTE: Cropping forward must be done first in order to keep target_seg_idx. + const auto cropped_forward_points = + cropForwardPoints(points, target_pos, target_seg_idx, forward_length); + + const size_t modified_target_seg_idx = + std::min(target_seg_idx, cropped_forward_points.size() - 2); + const auto cropped_points = cropBackwardPoints( + cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); + + if (cropped_points.size() < 2) { + RCLCPP_ERROR( + rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + ". Return original points since cropped_points size is less than 2."); + return points; + } + + return cropped_points; +} + +template +TrajectoryPoint convertToTrajectoryPoint(const T & point) +{ + TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + return traj_point; +} + +template <> +inline TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point) +{ + TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(ref_point); + traj_point.longitudinal_velocity_mps = tier4_autoware_utils::getLongitudinalVelocity(ref_point); + return traj_point; +} + +template +std::vector convertToTrajectoryPoints(const std::vector & points) +{ + std::vector traj_points; + for (const auto & point : points) { + const auto traj_point = convertToTrajectoryPoint(point); + traj_points.push_back(traj_point); + } + return traj_points; +} + +ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point); +std::vector convertToReferencePoints( + const std::vector & traj_points); + +void compensateLastPose( + const PathPoint & last_path_point, std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold); + +geometry_msgs::msg::Point getNearestPosition( + const std::vector & points, const int target_idx, const double offset); + +template +size_t findEgoIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, + const EgoNearestParam & ego_nearest_param) +{ + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); +} + +template +size_t findEgoSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, + const EgoNearestParam & ego_nearest_param) +{ + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & traj_points); + +std::vector resampleTrajectoryPoints( + const std::vector traj_points, const double interval); + +std::vector resampleTrajectoryPointsWithoutStopPoint( + const std::vector traj_points, const double interval); + +std::vector resampleReferencePoints( + const std::vector ref_points, const double interval); + +template +std::optional updateFrontPointForFix( + std::vector & points, std::vector & points_for_fix, const double delta_arc_length, + const EgoNearestParam & ego_nearest_param) +{ + // calculate front point to insert in points as a fixed point + const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( + points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + const size_t front_point_idx_for_fix = front_seg_idx_for_fix; + const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); + + // check if the points_for_fix is longer in front than points + const double lon_offset_to_prev_front = + motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + if (0 < lon_offset_to_prev_front) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + "Fixed point will not be inserted due to the error during calculation."); + return std::nullopt; + } + + const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + + // check if deviation is not too large + constexpr double max_lat_error = 3.0; + if (max_lat_error < dist) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + "New Fixed point is too far from points %f [m]", dist); + } + + // update pose + if (dist < delta_arc_length) { + // only pose is updated + points.front() = front_fix_point; + } else { + // add new front point + T new_front_point; + new_front_point = front_fix_point; + points.insert(points.begin(), new_front_point); + } + + return front_point_idx_for_fix; +} + +void insertStopPoint( + std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, + const size_t stop_seg_idx); +} // namespace trajectory_utils +} // namespace obstacle_avoidance_planner +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp deleted file mode 100644 index e6c6c2d4dc467..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp +++ /dev/null @@ -1,366 +0,0 @@ -// Copyright 2020 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 OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ - -#include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" - -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" - -#include "boost/optional/optional_fwd.hpp" - -#include -#include -#include -#include -#include - -struct ReferencePoint; - -namespace tier4_autoware_utils -{ -template <> -geometry_msgs::msg::Point getPoint(const ReferencePoint & p); - -template <> -geometry_msgs::msg::Pose getPose(const ReferencePoint & p); -} // namespace tier4_autoware_utils - -namespace geometry_utils -{ -template -geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const T & point, const geometry_msgs::msg::Pose & origin) -{ - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point relative_p; - const double tmp_x = point.x - origin.position.x; - const double tmp_y = point.y - origin.position.y; - relative_p.x = tmp_x * cos_yaw + tmp_y * sin_yaw; - relative_p.y = -tmp_x * sin_yaw + tmp_y * cos_yaw; - relative_p.z = point.z; - - return relative_p; -} - -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); - -template -geometry_msgs::msg::Point transformMapToImage( - const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - double resolution = occupancy_grid_info.resolution; - double map_y_height = occupancy_grid_info.height; - double map_x_width = occupancy_grid_info.width; - double map_x_in_image_resolution = relative_p.x / resolution; - double map_y_in_image_resolution = relative_p.y / resolution; - geometry_msgs::msg::Point image_point; - image_point.x = map_y_height - map_y_in_image_resolution; - image_point.y = map_x_width - map_x_in_image_resolution; - return image_point; -} - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info); - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point); -} // namespace geometry_utils - -namespace interpolation_utils -{ -std::vector interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double offset); - -std::vector interpolateConnected2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double begin_yaw, const double end_yaw); - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, - const std::vector & base_yaw, const double resolution); - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution); - -template -std::vector getInterpolatedPoints( - const T & points, const double delta_arc_length, const double offset = 0) -{ - constexpr double epsilon = 1e-6; - std::vector tmp_x; - std::vector tmp_y; - for (size_t i = 0; i < points.size(); i++) { - const auto & current_point = tier4_autoware_utils::getPoint(points.at(i)); - if (i > 0) { - const auto & prev_point = tier4_autoware_utils::getPoint(points.at(i - 1)); - if ( - std::fabs(current_point.x - prev_point.x) < epsilon && - std::fabs(current_point.y - prev_point.y) < epsilon) { - continue; - } - } - tmp_x.push_back(current_point.x); - tmp_y.push_back(current_point.y); - } - - return interpolation_utils::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, offset); -} - -std::vector getInterpolatedTrajectoryPoints( - const std::vector & points, - const double delta_arc_length); - -std::vector getConnectedInterpolatedPoints( - const std::vector & points, - const double delta_arc_length, const double begin_yaw, const double end_yaw); -} // namespace interpolation_utils - -namespace points_utils -{ -template -size_t findForwardIndex(const T & points, const size_t begin_idx, const double forward_length) -{ - double sum_length = 0.0; - for (size_t i = begin_idx; i < points.size() - 1; ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - if (sum_length > forward_length) { - return i; - } - } - return points.size() - 1; -} - -template -T clipForwardPoints(const T & points, const size_t begin_idx, const double forward_length) -{ - if (points.empty()) { - return T{}; - } - - const size_t end_idx = findForwardIndex(points, begin_idx, forward_length); - return T{points.begin() + begin_idx, points.begin() + end_idx}; -} - -template -T clipBackwardPoints( - const T & points, const size_t target_idx, const double backward_length, - const double delta_length) -{ - if (points.empty()) { - return T{}; - } - - const int begin_idx = - std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); - return T{points.begin() + begin_idx, points.end()}; -} - -template -T clipBackwardPoints( - const T & points, const geometry_msgs::msg::Pose pose, const double backward_length, - const double delta_length, const double delta_yaw) -{ - if (points.empty()) { - return T{}; - } - - const auto target_idx_optional = - motion_utils::findNearestIndex(points, pose, std::numeric_limits::max(), delta_yaw); - - const size_t target_idx = target_idx_optional - ? *target_idx_optional - : motion_utils::findNearestIndex(points, pose.position); - - const int begin_idx = - std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); - return T{points.begin() + begin_idx, points.end()}; -} - -// NOTE: acceleration is not converted -template -std::vector convertToPoints(const std::vector & points) -{ - std::vector geom_points; - for (const auto & point : points) { - geom_points.push_back(tier4_autoware_utils::getPoint(point)); - } - return geom_points; -} - -template -std::vector convertToPoses(const std::vector & points) -{ - std::vector geom_points; - for (const auto & point : points) { - geom_points.push_back(tier4_autoware_utils::getPose(point)); - } - return geom_points; -} - -template -autoware_auto_planning_msgs::msg::TrajectoryPoint convertToTrajectoryPoint(const T & point) -{ - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); - traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_point.lateral_velocity_mps = point.lateral_velocity_mps; - traj_point.heading_rate_rps = point.heading_rate_rps; - return traj_point; -} - -template <> -inline autoware_auto_planning_msgs::msg::TrajectoryPoint convertToTrajectoryPoint( - const ReferencePoint & point) -{ - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); - return traj_point; -} - -// functions to convert to another type of points -template -std::vector convertToTrajectoryPoints( - const std::vector & points) -{ - std::vector traj_points; - for (const auto & point : points) { - const auto traj_point = convertToTrajectoryPoint(point); - traj_points.push_back(traj_point); - } - return traj_points; -} - -template -ReferencePoint convertToReferencePoint(const T & point); - -template -std::vector convertToReferencePoints(const std::vector & points) -{ - std::vector ref_points; - for (const auto & point : points) { - const auto ref_point = convertToReferencePoint(point); - ref_points.push_back(ref_point); - } - return ref_points; -} - -std::vector convertToPosesWithYawEstimation( - const std::vector points); - -std::vector concatTrajectory( - const std::vector & traj_points, - const std::vector & extended_traj_points); - -void compensateLastPose( - const autoware_auto_planning_msgs::msg::PathPoint & last_path_point, - std::vector & traj_points, - const double delta_dist_threshold, const double delta_yaw_threshold); - -template -std::vector calcCurvature(const T & points, const size_t num_sampling_points) -{ - std::vector res(points.size()); - const size_t num_points = static_cast(points.size()); - - /* calculate curvature by circle fitting from three points */ - geometry_msgs::msg::Point p1, p2, p3; - size_t max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); - size_t L = std::min(num_sampling_points, max_smoothing_num); - for (size_t i = L; i < num_points - L; ++i) { - p1 = tier4_autoware_utils::getPoint(points.at(i - L)); - p2 = tier4_autoware_utils::getPoint(points.at(i)); - p3 = tier4_autoware_utils::getPoint(points.at(i + L)); - double den = std::max( - tier4_autoware_utils::calcDistance2d(p1, p2) * tier4_autoware_utils::calcDistance2d(p2, p3) * - tier4_autoware_utils::calcDistance2d(p3, p1), - 0.0001); - const double curvature = - 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; - res.at(i) = curvature; - } - - /* first and last curvature is copied from next value */ - for (size_t i = 0; i < std::min(L, num_points); ++i) { - res.at(i) = res.at(std::min(L, num_points - 1)); - res.at(num_points - i - 1) = - res.at(std::max(static_cast(num_points) - static_cast(L) - 1, 0)); - } - return res; -} - -int getNearestIdx( - const std::vector & points, const double target_s, const int begin_idx); - -template -bool isNearLastPathPoint( - const T & ref_point, const std::vector & path_points, - const double delta_dist_threshold, const double delta_yaw_threshold) -{ - const geometry_msgs::msg::Pose last_ref_pose = tier4_autoware_utils::getPose(ref_point); - - if ( - tier4_autoware_utils::calcDistance2d(last_ref_pose, path_points.back().pose) > - delta_dist_threshold) { - return false; - } - if ( - std::fabs(tier4_autoware_utils::calcYawDeviation(last_ref_pose, path_points.back().pose)) > - delta_yaw_threshold) { - return false; - } - return true; -} -} // namespace points_utils - -namespace utils -{ -void logOSQPSolutionStatus(const int solution_status, const std::string & msg); -} // namespace utils - -namespace drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector & left_bound, - const std::vector & right_bound, const VehicleParam & vehicle_param, - const bool is_considering_footprint_edges); -} - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 6cce41c70f227..13fd2995befb0 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 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. @@ -12,39 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -/** - * @file vehicle_model_bicycle_dynamics.h - * @brief vehicle model class of bicycle kinematics - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -/* - * Representation - * e : lateral error - * th : heading angle error - * steer : steering angle - * steer_d: desired steering angle (input) - * v : velocity - * W : wheelbase length - * tau : time constant for steering dynamics - * - * State & Input - * x = [e, th, steer]^T - * u = steer_d - * - * Nonlinear model - * dx1/dt = v * sin(x2) - * dx2/dt = v * tan(x3) / W - * dx3/dt = -(x3 - u) / tau - * - * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) - * [0, vr, 0] [ 0] [ 0] - * dx/dt = [0, 0, vr/W/cos(steer_r)^2] * x + [ 0] * u + [-vr*steer_r/W/cos(steer_r)^2] - * [0, 0, 1/tau] [1/tau] [ 0] - * - */ - #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ @@ -54,51 +21,14 @@ #include -/** - * @class vehicle model class of bicycle kinematics - * @brief calculate model-related values - */ class KinematicsBicycleModel : public VehicleModelInterface { public: - /** - * @brief constructor with parameter initialization - * @param [in] wheelbase wheelbase length [m] - * @param [in] steer_lim steering angle limit [rad] - */ - KinematicsBicycleModel(const double wheel_base, const double steer_limit); - - /** - * @brief destructor - */ + KinematicsBicycleModel(const double wheelbase, const double steer_limit); virtual ~KinematicsBicycleModel() = default; - /** - * @brief calculate discrete kinematics equation matrix of x_k+1 = Ad * x_k + Bd * uk + Wd - * @param [out] Ad coefficient matrix - * @param [out] Bd coefficient matrix - * @param [out] Wd coefficient matrix - * @param [in] ds discretization arc length - */ void calculateStateEquationMatrix( - Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) override; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd coefficient matrix - */ - void calculateObservationMatrix(Eigen::MatrixXd & Cd) override; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd_vec sparse matrix information of coefficient matrix - */ - void calculateObservationSparseMatrix(std::vector> & Cd_vec) override; - - /** - * @brief calculate reference input - * @param [out] reference input - */ - void calculateReferenceInput(Eigen::MatrixXd * Uref) override; + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double curvature, + const double ds) const override; }; #endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp deleted file mode 100644 index 291f35c0e15bb..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// 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. - -/** - * @file vehicle_model_bicycle_dynamics_no_delay.h - * @brief vehicle model class of bicycle kinematics without steering delay - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -/* - * Representation - * e : lateral error - * th : heading angle error - * steer : steering angle (input) - * v : velocity - * W : wheelbase length - * - * State & Input - * x = [e, th]^T - * u = steer - * - * Nonlinear model - * dx1/dt = v * sin(x2) - * dx2/dt = v * tan(u) / W - * - * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) - * dx/dt = [0, vr] * x + [ 0] * u + [ 0] - * [0, 0] [vr/W/cos(steer_r)^2] [-vr*steer_r/W/cos(steer_r)^2] - * - */ - -#ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ - -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" - -#include -#include - -/** - * @class vehicle model class of bicycle kinematics without steering delay - * @brief calculate model-related values - */ -class KinematicsBicycleModelNoDelay : public VehicleModelInterface -{ -public: - /** - * @brief constructor with parameter initialization - * @param [in] wheelbase wheelbase length [m] - * @param [in] steer_lim steering angle limit [rad] - */ - KinematicsBicycleModelNoDelay(const double & wheelbase, const double & steer_lim); - - /** - * @brief destructor - */ - ~KinematicsBicycleModelNoDelay() = default; - - /** - * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk - * @param [out] Ad coefficient matrix - * @param [out] Bd coefficient matrix - * @param [out] Cd coefficient matrix - * @param [out] Wd coefficient matrix - * @param [in] dt Discretization time - */ - void calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) override; - - /** - * @brief calculate reference input - * @param [out] reference input - */ - void calculateReferenceInput(Eigen::MatrixXd * Uref) override; - -private: - double wheelbase_; //!< @brief wheelbase length [m] - double steer_lim_; //!< @brief steering angle limit [rad] -}; -#endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp index 55de7dadc3d03..77dbaa80c7bbe 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 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. @@ -12,13 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -/** - * @file vehicle_model_interface.h - * @brief vehicle model interface class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ @@ -27,89 +20,28 @@ #include -/** - * @class vehicle model class - * @brief calculate model-related values - */ class VehicleModelInterface { protected: - const int dim_x_; // !< @brief dimension of kinematics x - const int dim_u_; // !< @brief dimension of input u - const int dim_y_; // !< @brief dimension of output y - double velocity_; // !< @brief vehicle velocity - double curvature_; // !< @brief curvature on the linearized point on path - double wheel_base_; // !< @brief wheel base of vehicle - double steer_limit_; // !< @brief vehicle velocity - double center_offset_from_base_; // !< @brief length from base lin to optimization center [m] + const int dim_x_; + const int dim_u_; + const int dim_y_; + const double wheelbase_; + const double steer_limit_; public: - /** - * @brief constructor - * @param [in] dim_x dimension of kinematics x - * @param [in] dim_u dimension of input u - * @param [in] dim_y dimension of output y - */ - VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheel_base, double steer_limit); - - /** - * @brief destructor - */ + VehicleModelInterface( + const int dim_x, const int dim_u, const int dim_y, const double wheelbase, + const double steer_limit); virtual ~VehicleModelInterface() = default; - /** - * @brief get kinematics x dimension - * @return kinematics dimension - */ - int getDimX(); - - /** - * @brief get input u dimension - * @return input dimension - */ - int getDimU(); - - /** - * @brief get output y dimension - * @return output dimension - */ - int getDimY(); - - void updateCenterOffset(const double center_offset_from_base); + int getDimX() const; + int getDimU() const; + int getDimY() const; - /** - * @brief set curvature - * @param [in] curvature curvature on the linearized point on path - */ - void setCurvature(const double curvature); - - /** - * @brief calculate discrete kinematics equation matrix of x_k+1 = Ad * x_k + Bd * uk + Wd - * @param [out] Ad coefficient matrix - * @param [out] Bd coefficient matrix - * @param [out] Wd coefficient matrix - * @param [in] ds discretization arc length - */ virtual void calculateStateEquationMatrix( - Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) = 0; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd coefficient matrix - */ - virtual void calculateObservationMatrix(Eigen::MatrixXd & Cd) = 0; - - /** - * @brief calculate discrete observation matrix of y_k = Cd * x_k - * @param [out] Cd_vec sparse matrix information of coefficient matrix - */ - virtual void calculateObservationSparseMatrix(std::vector> & Cd_vec) = 0; - - /** - * @brief calculate reference input - * @param [out] reference input - */ - virtual void calculateReferenceInput(Eigen::MatrixXd * Uref) = 0; + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double k, + const double ds) const = 0; }; #endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml b/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml index b35969aadbc6c..5a64fc970c867 100644 --- a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml +++ b/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml @@ -1,16 +1,15 @@ - - - + + - + - + diff --git a/planning/obstacle_avoidance_planner/media/bounds.svg b/planning/obstacle_avoidance_planner/media/bounds.svg new file mode 100644 index 0000000000000..1655ef6cb2818 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/bounds.svg @@ -0,0 +1,190 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ upper bound +
+
+
+ + upper bound + + + + + + + +
+
+
+ lower bound +
+
+
+ + lower bound + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `u_... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/media/eb.svg b/planning/obstacle_avoidance_planner/media/eb.svg new file mode 100644 index 0000000000000..452332cf4b5f2 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/eb.svg @@ -0,0 +1,371 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `p_k` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `p_{k... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `p_{k... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `(p_{k+1}-... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/media/eb_constraint.svg b/planning/obstacle_avoidance_planner/media/eb_constraint.svg new file mode 100644 index 0000000000000..ef91561297b86 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/eb_constraint.svg @@ -0,0 +1,991 @@ + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{k}^l`, `y_{... + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{k}`, `y_{k}` + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `sin(theta_k)... + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `sin(theta_k)... + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `sin(theta_k)... + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{k}^u`, `y_{... + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `theta_{k}` + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/media/vehicle_circles.svg b/planning/obstacle_avoidance_planner/media/vehicle_circles.svg new file mode 100644 index 0000000000000..4efa37daf4bd3 --- /dev/null +++ b/planning/obstacle_avoidance_planner/media/vehicle_circles.svg @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ (when n = 3) +
+
+
+ + (when n = 3) + + + + + + +
+
+
+ (when n = 3) +
+
+
+ + (when n = 3) + + + + + + +
+
+
+ Bicycle Model +
+
+
+ + Bicycle Model + + + + + + +
+
+
+ Uniform Circles +
+
+
+ + Uniform Circles + + + + + + +
+
+
+ Fitting Uniform Circles +
+
+
+ + Fitting Uniform Circl... + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index ad9dbd2c98f73..5f2e036847358 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -5,7 +5,6 @@ 0.1.0 The obstacle_avoidance_planner package Takayuki Murooka - Kosuke Takeuchi Apache License 2.0 Takayuki Murooka @@ -14,7 +13,6 @@ autoware_cmake - autoware_auto_perception_msgs autoware_auto_planning_msgs geometry_msgs interpolation @@ -31,8 +29,12 @@ vehicle_info_util visualization_msgs + ament_cmake_ros + ament_index_python ament_lint_auto autoware_lint_common + autoware_testing + fake_test_node ament_cmake diff --git a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py b/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py similarity index 75% rename from planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py rename to planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py index 908cc204da2e5..275ae69ef177c 100644 --- a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py +++ b/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# Copyright 2023 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. @@ -53,23 +53,42 @@ def __init__(self, args): def CallbackCalculationCost(self, msg): max_y = 0 max_x = 0 + for f_idx, function_name in enumerate(self.functions_name): + is_found = False for line in msg.data.split("\n"): if function_name in line: y = float(line.split(":=")[1].split("[ms]")[0]) self.y_vec[f_idx].append(y) + is_found = True + break + + if not is_found: + self.y_vec[f_idx].append(None) + if len(self.y_vec[f_idx]) > 100: self.y_vec[f_idx].popleft() + print(len(self.y_vec[f_idx])) + x_vec = list(range(len(self.y_vec[f_idx]))) - self.lines[f_idx].set_xdata(x_vec) - self.lines[f_idx].set_ydata(self.y_vec[f_idx]) + valid_x_vec = [] + valid_y_vec = [] + for i in range(len(x_vec)): + if self.y_vec[f_idx][i] is not None: + valid_x_vec.append(x_vec[i]) + valid_y_vec.append(self.y_vec[f_idx][i]) + + print(len(valid_x_vec), len(valid_y_vec)) + + self.lines[f_idx].set_xdata(valid_x_vec) + self.lines[f_idx].set_ydata(valid_y_vec) - if len(self.y_vec[f_idx]) > 0: - max_x = max(max_x, max(x_vec)) - max_y = max(max_y, max(self.y_vec[f_idx])) + if len(valid_y_vec) > 0: + max_x = max(max_x, max(valid_x_vec)) + max_y = max(max_y, max(valid_y_vec)) plt.xlim(0, max_x) plt.ylim(0.0, max_y) diff --git a/planning/obstacle_avoidance_planner/scripts/trajectory_visualizer.py b/planning/obstacle_avoidance_planner/scripts/trajectory_visualizer.py deleted file mode 100755 index b59643bca185a..0000000000000 --- a/planning/obstacle_avoidance_planner/scripts/trajectory_visualizer.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2020 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. - -# TODO(kosuke murakami): write ros2 visualizer -# import rospy -# from tier4_planning_msgs.msg import Trajectory -# from tier4_planning_msgs.msg import TrajectoryPoint -# import matplotlib.pyplot as plt -# import numpy as np -# import tf -# from geometry_msgs.msg import Vector3 - - -# def quaternion_to_euler(quaternion): -# """Convert Quaternion to Euler Angles - -# quaternion: geometry_msgs/Quaternion -# euler: geometry_msgs/Vector3 -# """ -# e = tf.transformations.euler_from_quaternion( -# (quaternion.x, quaternion.y, quaternion.z, quaternion.w)) -# return Vector3(x=e[0], y=e[1], z=e[2]) - - -# class TrajectoryVisualizer(): - -# def __init__(self): -# self.in_trajectory = Trajectory() -# self.debug_trajectory = Trajectory() -# self.debug_fixed_trajectory = Trajectory() - -# self.plot_done1 = True -# self.plot_done2 = True -# self.plot_done3 = True - -# self.length = 50 - -# self.sub_status1 = rospy.Subscriber( -# "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", -# Trajectory, self.CallBackTraj, queue_size=1, tcp_nodelay=True) -# rospy.Timer(rospy.Duration(0.3), self.timerCallback) - -# def CallBackTraj(self, cmd): -# if (self.plot_done1): -# self.in_trajectory = cmd -# self.plot_done1 = False - -# def CallBackDebugTraj(self, cmd): -# if (self.plot_done2): -# self.debug_trajectory = cmd -# self.plot_done2 = False - -# def CallBackDebugFixedTraj(self, cmd): -# if (self.plot_done3): -# self.debug_fixed_trajectory = cmd -# self.plot_done3 = False - -# def timerCallback(self, event): -# self.plotTrajectory() -# self.plot_done1 = True -# self.plot_done2 = True -# self.plot_done3 = True - -# def CalcArcLength(self, traj): -# s_arr = [] -# ds = 0.0 -# s_sum = 0.0 - -# if len(traj.points) > 0: -# s_arr.append(s_sum) - -# for i in range(1, len(traj.points)): -# p0 = traj.points[i-1] -# p1 = traj.points[i] -# dx = p1.pose.position.x - p0.pose.position.x -# dy = p1.pose.position.y - p0.pose.position.y -# ds = np.sqrt(dx**2 + dy**2) -# s_sum += ds -# if(s_sum > self.length): -# break -# s_arr.append(s_sum) -# return s_arr - -# def CalcX(self, traj): -# v_list = [] -# for p in traj.points: -# v_list.append(p.pose.position.x) -# return v_list - -# def CalcY(self, traj): -# v_list = [] -# for p in traj.points: -# v_list.append(p.pose.position.y) -# return v_list - -# def CalcYaw(self, traj, s_arr): -# v_list = [] -# for p in traj.points: -# v_list.append(quaternion_to_euler(p.pose.orientation).z) -# return v_list[0: len(s_arr)] - -# def plotTrajectory(self): -# plt.clf() -# ax3 = plt.subplot(1, 1, 1) -# x = self.CalcArcLength(self.in_trajectory) -# y = self.CalcYaw(self.in_trajectory, x) -# if len(x) == len(y): -# ax3.plot(x, y, label="final", marker="*") -# ax3.set_xlabel("arclength [m]") -# ax3.set_ylabel("yaw") -# plt.pause(0.01) - - -# def main(): -# rospy.init_node("trajectory_visualizer") -# TrajectoryVisualizer() -# rospy.spin() - - -# if __name__ == "__main__": -# main() diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp new file mode 100644 index 0000000000000..c517ade57c6c4 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -0,0 +1,344 @@ +// Copyright 2023 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 "obstacle_avoidance_planner/debug_marker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" + +#include "visualization_msgs/msg/marker_array.hpp" + +namespace obstacle_avoidance_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +namespace +{ +MarkerArray getFootprintsMarkerArray( + const std::vector & mpt_traj, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "mpt_footprints", 0, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.99, 0.99, 0.2, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + for (size_t i = 0; i < mpt_traj.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + + marker.id = i; + marker.points.clear(); + + const auto & traj_point = mpt_traj.at(i); + + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0) + .position); + marker.points.push_back(marker.points.front()); + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray getBoundsLineMarkerArray( + const std::vector & ref_points, const double vehicle_width, + const size_t sampling_num) +{ + const auto current_time = rclcpp::Clock().now(); + MarkerArray marker_array; + + if (ref_points.empty()) return marker_array; + + // create lower bound marker + auto lb_marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "", 0, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(0.99 + 0.5, 0.99, 0.2, 0.3)); + lb_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + // create upper bound marker + auto ub_marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "", 1, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(0.99, 0.99 + 0.5, 0.2, 0.3)); + ub_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + for (size_t bound_idx = 0; bound_idx < ref_points.at(0).bounds_on_constraints.size(); + ++bound_idx) { + const std::string ns = "base_bounds_" + std::to_string(bound_idx); + + { // lower bound + lb_marker.points.clear(); + lb_marker.ns = ns; + + for (size_t i = 0; i < ref_points.size(); i++) { + if (i % sampling_num != 0) { + continue; + } + + const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double lb_y = + ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - vehicle_width / 2.0; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + + lb_marker.points.push_back(pose.position); + lb_marker.points.push_back(lb); + } + marker_array.markers.push_back(lb_marker); + } + + { // upper bound + ub_marker.points.clear(); + ub_marker.ns = ns; + + for (size_t i = 0; i < ref_points.size(); i++) { + if (i % sampling_num != 0) { + continue; + } + + const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double ub_y = + ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + vehicle_width / 2.0; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + + ub_marker.points.push_back(pose.position); + ub_marker.points.push_back(ub); + } + marker_array.markers.push_back(ub_marker); + } + } + + return marker_array; +} + +MarkerArray getVehicleCircleLinesMarkerArray( + const std::vector & ref_points, + const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, + const size_t sampling_num, const std::string & ns) +{ + const auto current_time = rclcpp::Clock().now(); + MarkerArray msg; + + for (size_t i = 0; i < ref_points.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + const auto & ref_point = ref_points.at(i); + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, i, Marker::LINE_LIST, createMarkerScale(0.1, 0, 0), + createMarkerColor(0.99, 0.99, 0.2, 0.25)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + const double lat_dev = ref_point.optimized_kinematic_state.lat; + const double yaw_dev = ref_point.optimized_kinematic_state.yaw; + + // apply lateral and yaw deviation + auto pose_with_deviation = + tier4_autoware_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); + pose_with_deviation.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); + + for (const double d : vehicle_circle_longitudinal_offsets) { + // apply longitudinal offset + auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); + base_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); + + const auto ub = + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, vehicle_width / 2.0, 0.0).position; + const auto lb = + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -vehicle_width / 2.0, 0.0).position; + + marker.points.push_back(ub); + marker.points.push_back(lb); + } + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray getCurrentVehicleCirclesMarkerArray( + const geometry_msgs::msg::Pose & ego_pose, + const std::vector & vehicle_circle_longitudinal_offsets, + const std::vector & vehicle_circle_radiuses, const std::string & ns, const double r, + const double g, const double b) +{ + MarkerArray msg; + + size_t id = 0; + for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { + const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + marker.pose = tier4_autoware_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); + + constexpr size_t circle_dividing_num = 16; + for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { + geometry_msgs::msg::Point edge_pos; + + const double edge_angle = + static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; + edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle); + + marker.points.push_back(edge_pos); + } + + msg.markers.push_back(marker); + ++id; + } + return msg; +} + +MarkerArray getVehicleCirclesMarkerArray( + const std::vector & mpt_traj_points, + const std::vector & vehicle_circle_longitudinal_offsets, + const std::vector & vehicle_circle_radiuses, const size_t sampling_num, + const std::string & ns, const double r, const double g, const double b) +{ + MarkerArray msg; + + size_t id = 0; + for (size_t i = 0; i < mpt_traj_points.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + const auto & mpt_traj_point = mpt_traj_points.at(i); + + for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { + const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), + createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); + + constexpr size_t circle_dividing_num = 16; + for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { + geometry_msgs::msg::Point edge_pos; + + const double edge_angle = + static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; + edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle); + + marker.points.push_back(edge_pos); + } + + msg.markers.push_back(marker); + ++id; + } + } + return msg; +} + +visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( + const std::vector & ref_points) +{ + if (ref_points.empty()) { + return visualization_msgs::msg::MarkerArray{}; + } + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "text", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.15), createMarkerColor(1.0, 1.0, 0.0, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + visualization_msgs::msg::MarkerArray msg; + for (size_t i = 0; i < ref_points.size(); i++) { + marker.id = i; + // marker.text = std::to_string(tf2::getYaw(ref_points[i].pose.orientation)) + "\n" + + // std::to_string(ref_points[i].delta_arc_length); marker.text = + // std::to_string(ref_points[i].alpha) + "\n" + std::to_string(ref_points[i].beta); marker.text + // marker.text = std::to_string(ref_points.at(i).curvature); + marker.text = std::to_string(ref_points.at(i).curvature) + " \n" + + std::to_string(ref_points.at(i).optimized_input); + marker.pose.position = ref_points.at(i).pose.position; + msg.markers.push_back(marker); + } + + return msg; +} +} // namespace + +MarkerArray getDebugMarker( + const DebugData & debug_data, const std::vector & optimized_points, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + MarkerArray marker_array; + + // mpt footprints + appendMarkerArray( + getFootprintsMarkerArray(optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), + &marker_array); + + // bounds + appendMarkerArray( + getBoundsLineMarkerArray( + debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), + &marker_array); + + // vehicle circle line + appendMarkerArray( + getVehicleCircleLinesMarkerArray( + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, + vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), + &marker_array); + + // current vehicle circles + appendMarkerArray( + getCurrentVehicleCirclesMarkerArray( + debug_data.ego_pose, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), + &marker_array); + + // vehicle circles + appendMarkerArray( + getVehicleCirclesMarkerArray( + optimized_points, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles", + 1.0, 0.3, 0.3), + &marker_array); + + // debug text + appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); + + return marker_array; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp deleted file mode 100644 index d26180f604e88..0000000000000 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ /dev/null @@ -1,635 +0,0 @@ -// Copyright 2020 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 "obstacle_avoidance_planner/eb_path_optimizer.hpp" - -#include "obstacle_avoidance_planner/utils/utils.hpp" - -#include "geometry_msgs/msg/vector3.hpp" - -#include -#include -#include -#include -#include - -EBPathOptimizer::EBPathOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, const EBParam & eb_param, - const VehicleParam & vehicle_param) -: is_showing_debug_info_(is_showing_debug_info), - epsilon_(1e-8), - qp_param_(eb_param.qp_param), - traj_param_(traj_param), - eb_param_(eb_param), - vehicle_param_(vehicle_param) -{ - const Eigen::MatrixXd p = makePMatrix(); - default_a_matrix_ = makeAMatrix(); - - const int num_points = eb_param_.num_sampling_points_for_eb; - const std::vector q(num_points * 2, 0.0); - const std::vector lower_bound(num_points * 2, 0.0); - const std::vector upper_bound(num_points * 2, 0.0); - - osqp_solver_ptr_ = std::make_unique( - p, default_a_matrix_, q, lower_bound, upper_bound, qp_param_.eps_abs); - osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - osqp_solver_ptr_->updateMaxIter(qp_param_.max_iteration); -} - -// make positive semidefinite matrix for objective function -// reference: https://ieeexplore.ieee.org/document/7402333 -Eigen::MatrixXd EBPathOptimizer::makePMatrix() -{ - const int num_points = eb_param_.num_sampling_points_for_eb; - - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(num_points * 2, num_points * 2); - for (int r = 0; r < num_points * 2; ++r) { - for (int c = 0; c < num_points * 2; ++c) { - if (r == c) { - P(r, c) = 6.0; - } else if (std::abs(c - r) == 1) { - P(r, c) = -4.0; - } else if (std::abs(c - r) == 2) { - P(r, c) = 1.0; - } else { - P(r, c) = 0.0; - } - } - } - return P; -} - -// make default linear constrain matrix -Eigen::MatrixXd EBPathOptimizer::makeAMatrix() -{ - const int num_points = eb_param_.num_sampling_points_for_eb; - - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(num_points * 2, num_points * 2); - for (int i = 0; i < num_points * 2; ++i) { - if (i < num_points) { - A(i, i + num_points) = 1.0; - } else { - A(i, i - num_points) = 1.0; - } - } - return A; -} - -boost::optional> -EBPathOptimizer::getEBTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, const double current_ego_vel, - DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - current_ego_vel_ = current_ego_vel; - - // get candidate points for optimization - // decide fix or non fix, might not required only for smoothing purpose - const CandidatePoints candidate_points = - getCandidatePoints(ego_pose, path.points, prev_trajs, debug_data); - if (candidate_points.fixed_points.empty() && candidate_points.non_fixed_points.empty()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "return boost::none since empty candidate points"); - return boost::none; - } - - // get optimized smooth points with elastic band - const auto eb_traj_points = getOptimizedTrajectory(path, candidate_points, debug_data); - if (!eb_traj_points) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "return boost::none since smoothing failed"); - return boost::none; - } - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return eb_traj_points; -} - -boost::optional> -EBPathOptimizer::getOptimizedTrajectory( - const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, - DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - // get constrain rectangles around each point - auto full_points = candidate_points.fixed_points; - full_points.insert( - full_points.end(), candidate_points.non_fixed_points.begin(), - candidate_points.non_fixed_points.end()); - - // interpolate points for logic purpose - std::vector interpolated_points = - interpolation_utils::getInterpolatedPoints(full_points, eb_param_.delta_arc_length_for_eb); - if (interpolated_points.empty()) { - return boost::none; - } - - // clip interpolated points with the length of path - if (traj_param_.enable_clipping_fixed_traj) { - if (path.points.size() < 2) { - return boost::none; - } - const auto interpolated_poses = - points_utils::convertToPosesWithYawEstimation(interpolated_points); - const auto interpolated_points_end_seg_idx = motion_utils::findNearestSegmentIndex( - interpolated_poses, path.points.back().pose, 3.0, 0.785); - if (interpolated_points_end_seg_idx) { - interpolated_points = std::vector( - interpolated_points.begin(), - interpolated_points.begin() + interpolated_points_end_seg_idx.get()); - if (interpolated_points.empty()) { - return boost::none; - } - } - } - - debug_data.interpolated_points = interpolated_points; - // number of optimizing points - const int farthest_idx = std::min( - (eb_param_.num_sampling_points_for_eb - 1), static_cast(interpolated_points.size() - 1)); - // number of fixed points in interpolated_points - const int num_fixed_points = - getNumFixedPoints(candidate_points.fixed_points, interpolated_points, farthest_idx); - // TODO(murooka) try this printing. something may be wrong - // std::cerr << num_fixed_points << std::endl; - - // consider straight after `straight_line_idx` and then tighten space for optimization after - // `straight_line_idx` - const int straight_line_idx = - getStraightLineIdx(interpolated_points, farthest_idx, debug_data.straight_points); - - // if `farthest_idx` is lower than `number_of_sampling_points`, duplicate the point at the end of - // `interpolated_points` - // This aims for using hotstart by not changing the size of matrix - std::vector padded_interpolated_points = - getPaddedInterpolatedPoints(interpolated_points, farthest_idx); - - const auto rectangles = getConstrainRectangleVec( - path, padded_interpolated_points, num_fixed_points, farthest_idx, straight_line_idx); - if (!rectangles) { - return boost::none; - } - - const auto traj_points = - calculateTrajectory(padded_interpolated_points, rectangles.get(), farthest_idx, debug_data); - if (!traj_points) { - return boost::none; - } - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return *traj_points; -} - -boost::optional> -EBPathOptimizer::calculateTrajectory( - const std::vector & padded_interpolated_points, - const std::vector & constrain_rectangles, const int farthest_idx, - DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - // update constrain for QP based on constrain rectangles - updateConstrain(padded_interpolated_points, constrain_rectangles); - - // solve QP and get optimized trajectory - const auto result = solveQP(); - const auto optimized_points = result.first; - const auto status = result.second; - if (status != 1) { - utils::logOSQPSolutionStatus(status, "EB: "); - return boost::none; - } - - const auto traj_points = - convertOptimizedPointsToTrajectory(optimized_points, constrain_rectangles, farthest_idx); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return traj_points; -} - -std::pair, int64_t> EBPathOptimizer::solveQP() -{ - osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs); - - const auto result = osqp_solver_ptr_->optimize(); - const auto optimized_points = std::get<0>(result); - const auto status = std::get<3>(result); - - utils::logOSQPSolutionStatus(std::get<3>(result), "EB: "); - - return std::make_pair(optimized_points, status); -} - -std::vector EBPathOptimizer::getFixedPoints( - const geometry_msgs::msg::Pose & ego_pose, - [[maybe_unused]] const std::vector & path_points, - const std::unique_ptr & prev_trajs) -{ - /* use of prev_traj_points(fine resolution) instead of prev_opt_traj(coarse resolution) - stabilize trajectory's yaw*/ - if (prev_trajs) { - if (prev_trajs->smoothed_trajectory.empty()) { - std::vector empty_points; - return empty_points; - } - const size_t begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - prev_trajs->smoothed_trajectory, ego_pose, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold); - const int backward_fixing_idx = std::max( - static_cast( - begin_idx - - traj_param_.backward_fixing_distance / traj_param_.delta_arc_length_for_trajectory), - 0); - - // NOTE: Fixed length of EB has to be longer than that of MPT. - constexpr double forward_fixed_length_margin = 5.0; - const double forward_fixed_length = std::max( - current_ego_vel_ * traj_param_.forward_fixing_min_time + forward_fixed_length_margin, - traj_param_.forward_fixing_min_distance); - - const int forward_fixing_idx = std::min( - static_cast( - begin_idx + forward_fixed_length / traj_param_.delta_arc_length_for_trajectory), - static_cast(prev_trajs->smoothed_trajectory.size() - 1)); - std::vector fixed_points; - for (int i = backward_fixing_idx; i <= forward_fixing_idx; i++) { - fixed_points.push_back(prev_trajs->smoothed_trajectory.at(i).pose); - } - return fixed_points; - } else { - std::vector empty_points; - return empty_points; - } -} - -EBPathOptimizer::CandidatePoints EBPathOptimizer::getCandidatePoints( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_trajs, DebugData & debug_data) -{ - const std::vector fixed_points = - getFixedPoints(ego_pose, path_points, prev_trajs); - if (fixed_points.empty()) { - CandidatePoints candidate_points = getDefaultCandidatePoints(path_points); - return candidate_points; - } - - // try to find non-fix points - const auto opt_begin_idx = motion_utils::findNearestIndex( - path_points, fixed_points.back(), std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - if (!opt_begin_idx) { - CandidatePoints candidate_points; - candidate_points.fixed_points = fixed_points; - candidate_points.begin_path_idx = path_points.size(); - candidate_points.end_path_idx = path_points.size(); - return candidate_points; - } - - const int begin_idx = std::min( - static_cast(opt_begin_idx.get()) + eb_param_.num_offset_for_begin_idx, - static_cast(path_points.size()) - 1); - - std::vector non_fixed_points; - for (size_t i = begin_idx; i < path_points.size(); i++) { - non_fixed_points.push_back(path_points[i].pose); - } - CandidatePoints candidate_points; - candidate_points.fixed_points = fixed_points; - candidate_points.non_fixed_points = non_fixed_points; - candidate_points.begin_path_idx = begin_idx; - candidate_points.end_path_idx = path_points.size() - 1; - - debug_data.fixed_points = candidate_points.fixed_points; - debug_data.non_fixed_points = candidate_points.non_fixed_points; - return candidate_points; -} - -std::vector EBPathOptimizer::getPaddedInterpolatedPoints( - const std::vector & interpolated_points, const int farthest_point_idx) -{ - std::vector padded_interpolated_points; - for (int i = 0; i < eb_param_.num_sampling_points_for_eb; i++) { - if (i > farthest_point_idx) { - padded_interpolated_points.push_back(interpolated_points[farthest_point_idx]); - } else { - padded_interpolated_points.push_back(interpolated_points[i]); - } - } - return padded_interpolated_points; -} - -EBPathOptimizer::CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( - const std::vector & path_points) -{ - double accum_arc_length = 0; - int end_path_idx = 0; - std::vector fixed_points; - for (size_t i = 0; i < path_points.size(); i++) { - if (i > 0) { - accum_arc_length += tier4_autoware_utils::calcDistance2d( - path_points[i].pose.position, path_points[i - 1].pose.position); - } - if ( - accum_arc_length > eb_param_.num_sampling_points_for_eb * eb_param_.delta_arc_length_for_eb) { - break; - } - end_path_idx = i; - fixed_points.push_back(path_points[i].pose); - } - CandidatePoints candidate_points; - candidate_points.fixed_points = fixed_points; - candidate_points.begin_path_idx = 0; - candidate_points.end_path_idx = end_path_idx; - return candidate_points; -} - -int EBPathOptimizer::getNumFixedPoints( - const std::vector & fixed_points, - const std::vector & interpolated_points, const int farthest_idx) -{ - int num_fixed_points = 0; - if (!fixed_points.empty() && !interpolated_points.empty()) { - std::vector interpolated_points = - interpolation_utils::getInterpolatedPoints(fixed_points, eb_param_.delta_arc_length_for_eb); - num_fixed_points = interpolated_points.size(); - } - num_fixed_points = std::min(num_fixed_points, farthest_idx); - return num_fixed_points; -} - -std::vector -EBPathOptimizer::convertOptimizedPointsToTrajectory( - const std::vector optimized_points, - [[maybe_unused]] const std::vector & constraints, const int farthest_idx) -{ - std::vector traj_points; - for (int i = 0; i <= farthest_idx; i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; - tmp_point.pose.position.x = optimized_points[i]; - tmp_point.pose.position.y = optimized_points[i + eb_param_.num_sampling_points_for_eb]; - traj_points.push_back(tmp_point); - } - for (size_t i = 0; i < traj_points.size(); i++) { - if (i > 0) { - traj_points[i].pose.orientation = geometry_utils::getQuaternionFromPoints( - traj_points[i].pose.position, traj_points[i - 1].pose.position); - } else if (i == 0 && traj_points.size() > 1) { - traj_points[i].pose.orientation = geometry_utils::getQuaternionFromPoints( - traj_points[i + 1].pose.position, traj_points[i].pose.position); - } - } - return traj_points; -} - -boost::optional> EBPathOptimizer::getConstrainRectangleVec( - const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx) -{ - auto curvatures = points_utils::calcCurvature(interpolated_points, 10); - - std::vector smooth_constrain_rects(eb_param_.num_sampling_points_for_eb); - for (int i = 0; i < eb_param_.num_sampling_points_for_eb; i++) { - // `Anchor` has pose + velocity - const Anchor anchor = getAnchor(interpolated_points, i, path.points); - - // four types of rectangle for various types - if (i == 0 || i == 1 || i >= farthest_point_idx - 1 || i < num_fixed_points - 1) { - // rect has four points for a rectangle in map coordinate - const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_fixing); - smooth_constrain_rects[i] = rect; - } else if ( // NOLINT - i >= num_fixed_points - eb_param_.num_joint_buffer_points && - i <= num_fixed_points + eb_param_.num_joint_buffer_points) { - const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_joint); - smooth_constrain_rects[i] = rect; - } else if (i >= straight_idx) { - const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_straight_line); - smooth_constrain_rects[i] = rect; - } else { - const double min_x = -eb_param_.clearance_for_only_smoothing; - const double max_x = eb_param_.clearance_for_only_smoothing; - const double min_y = curvatures[i] > 0 ? 0 : -eb_param_.clearance_for_only_smoothing; - const double max_y = curvatures[i] <= 0 ? 0 : eb_param_.clearance_for_only_smoothing; - const auto rect = getConstrainRectangle(anchor, min_x, max_x, min_y, max_y); - smooth_constrain_rects[i] = rect; - } - } - return smooth_constrain_rects; -} - -void EBPathOptimizer::updateConstrain( - const std::vector & interpolated_points, - const std::vector & rectangle_points) -{ - const int num_points = eb_param_.num_sampling_points_for_eb; - - Eigen::MatrixXd A = default_a_matrix_; - std::vector lower_bound(num_points * 2, 0.0); - std::vector upper_bound(num_points * 2, 0.0); - for (int i = 0; i < num_points; ++i) { - Constrain constrain = - getConstrainFromConstrainRectangle(interpolated_points[i], rectangle_points[i]); - A(i, i) = constrain.top_and_bottom.x_coef; - A(i, i + num_points) = constrain.top_and_bottom.y_coef; - A(i + num_points, i) = constrain.left_and_right.x_coef; - A(i + num_points, i + num_points) = constrain.left_and_right.y_coef; - lower_bound[i] = constrain.top_and_bottom.lower_bound; - upper_bound[i] = constrain.top_and_bottom.upper_bound; - lower_bound[i + num_points] = constrain.left_and_right.lower_bound; - upper_bound[i + num_points] = constrain.left_and_right.upper_bound; - } - - osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - osqp_solver_ptr_->updateA(A); -} - -EBPathOptimizer::Anchor EBPathOptimizer::getAnchor( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points) const -{ - geometry_msgs::msg::Pose pose; - pose.position = interpolated_points[interpolated_idx]; - if (interpolated_idx > 0) { - pose.orientation = geometry_utils::getQuaternionFromPoints( - interpolated_points[interpolated_idx], interpolated_points[interpolated_idx - 1]); - } else if (interpolated_idx == 0 && interpolated_points.size() > 1) { - pose.orientation = geometry_utils::getQuaternionFromPoints( - interpolated_points[interpolated_idx + 1], interpolated_points[interpolated_idx]); - } - const auto opt_nearest_idx = motion_utils::findNearestIndex( - path_points, pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - const int nearest_idx = opt_nearest_idx ? *opt_nearest_idx : 0; - - const geometry_msgs::msg::Quaternion nearest_q = path_points[nearest_idx].pose.orientation; - Anchor anchor; - anchor.pose.position = interpolated_points[interpolated_idx]; - anchor.pose.orientation = nearest_q; - return anchor; -} - -int EBPathOptimizer::getStraightLineIdx( - const std::vector & interpolated_points, const int farthest_point_idx, - std::vector & debug_detected_straight_points) -{ - double prev_yaw = 0; - int straight_line_idx = farthest_point_idx; - for (int i = farthest_point_idx; i >= 0; i--) { - if (i < farthest_point_idx) { - const double yaw = - tier4_autoware_utils::calcAzimuthAngle(interpolated_points[i], interpolated_points[i + 1]); - const double delta_yaw = yaw - prev_yaw; - const double norm_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); - if (std::fabs(norm_delta_yaw) > traj_param_.delta_yaw_threshold_for_straight) { - break; - } - straight_line_idx = i; - prev_yaw = yaw; - } else if (i == farthest_point_idx && farthest_point_idx >= 1) { - const double yaw = - tier4_autoware_utils::calcAzimuthAngle(interpolated_points[i - 1], interpolated_points[i]); - prev_yaw = yaw; - } - } - for (int i = straight_line_idx; i <= farthest_point_idx; i++) { - debug_detected_straight_points.push_back(interpolated_points[i]); - } - return straight_line_idx; -} - -EBPathOptimizer::Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( - const geometry_msgs::msg::Point & interpolated_point, const ConstrainRectangle & constrain_range) -{ - Constrain constrain; - const double top_dx = constrain_range.top_left.x - constrain_range.top_right.x; - const double top_dy = constrain_range.top_left.y - constrain_range.top_right.y; - const double left_dx = constrain_range.top_left.x - constrain_range.bottom_left.x; - const double left_dy = constrain_range.top_left.y - constrain_range.bottom_left.y; - if ( - std::fabs(top_dx) < epsilon_ && std::fabs(top_dy) < epsilon_ && std::fabs(left_dx) < epsilon_ && - std::fabs(left_dy) < epsilon_) { - constrain.top_and_bottom.x_coef = 1; - constrain.top_and_bottom.y_coef = 1; - constrain.top_and_bottom.lower_bound = interpolated_point.x + interpolated_point.y; - constrain.top_and_bottom.upper_bound = interpolated_point.x + interpolated_point.y; - constrain.left_and_right.x_coef = -1; - constrain.left_and_right.y_coef = 1; - constrain.left_and_right.lower_bound = interpolated_point.y - interpolated_point.x; - constrain.left_and_right.upper_bound = interpolated_point.y - interpolated_point.x; - } else if (std::fabs(top_dx) < epsilon_) { - constrain.top_and_bottom.x_coef = 1; - constrain.top_and_bottom.y_coef = epsilon_; - constrain.top_and_bottom.lower_bound = interpolated_point.x; - constrain.top_and_bottom.upper_bound = interpolated_point.x; - constrain.left_and_right = - getConstrainLines(left_dx, left_dy, constrain_range.top_left, constrain_range.top_right); - } else if (std::fabs(top_dy) < epsilon_) { - constrain.top_and_bottom.x_coef = epsilon_; - constrain.top_and_bottom.y_coef = 1; - constrain.top_and_bottom.lower_bound = interpolated_point.y; - constrain.top_and_bottom.upper_bound = interpolated_point.y; - constrain.left_and_right = - getConstrainLines(left_dx, left_dy, constrain_range.top_left, constrain_range.top_right); - } else if (std::fabs(left_dx) < epsilon_) { - constrain.left_and_right.x_coef = 1; - constrain.left_and_right.y_coef = epsilon_; - constrain.left_and_right.lower_bound = interpolated_point.x; - constrain.left_and_right.upper_bound = interpolated_point.x; - constrain.top_and_bottom = - getConstrainLines(top_dx, top_dy, constrain_range.top_left, constrain_range.bottom_left); - } else if (std::fabs(left_dy) < epsilon_) { - constrain.left_and_right.x_coef = epsilon_; - constrain.left_and_right.y_coef = 1; - constrain.left_and_right.lower_bound = interpolated_point.y; - constrain.left_and_right.upper_bound = interpolated_point.y; - constrain.top_and_bottom = - getConstrainLines(top_dx, top_dy, constrain_range.top_left, constrain_range.bottom_left); - } else { - constrain.top_and_bottom = - getConstrainLines(top_dx, top_dy, constrain_range.top_left, constrain_range.bottom_left); - constrain.left_and_right = - getConstrainLines(left_dx, left_dy, constrain_range.top_left, constrain_range.top_right); - } - return constrain; -} - -EBPathOptimizer::ConstrainLines EBPathOptimizer::getConstrainLines( - const double dx, const double dy, const geometry_msgs::msg::Point & point, - const geometry_msgs::msg::Point & opposite_point) -{ - ConstrainLines constrain_point; - - const double slope = dy / dx; - const double intercept = point.y - slope * point.x; - const double intercept2 = opposite_point.y - slope * opposite_point.x; - constrain_point.x_coef = -1 * slope; - constrain_point.y_coef = 1; - if (intercept > intercept2) { - constrain_point.lower_bound = intercept2; - constrain_point.upper_bound = intercept; - } else { - constrain_point.lower_bound = intercept; - constrain_point.upper_bound = intercept2; - } - return constrain_point; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const Anchor & anchor, const double clearance) const -{ - ConstrainRectangle constrain_range; - geometry_msgs::msg::Point top_left; - top_left.x = clearance; - top_left.y = clearance; - constrain_range.top_left = geometry_utils::transformToAbsoluteCoordinate2D(top_left, anchor.pose); - geometry_msgs::msg::Point top_right; - top_right.x = clearance; - top_right.y = -1 * clearance; - constrain_range.top_right = - geometry_utils::transformToAbsoluteCoordinate2D(top_right, anchor.pose); - geometry_msgs::msg::Point bottom_left; - bottom_left.x = -1 * clearance; - bottom_left.y = clearance; - constrain_range.bottom_left = - geometry_utils::transformToAbsoluteCoordinate2D(bottom_left, anchor.pose); - geometry_msgs::msg::Point bottom_right; - bottom_right.x = -1 * clearance; - bottom_right.y = -1 * clearance; - constrain_range.bottom_right = - geometry_utils::transformToAbsoluteCoordinate2D(bottom_right, anchor.pose); - return constrain_range; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const Anchor & anchor, const double min_x, const double max_x, const double min_y, - const double max_y) const -{ - ConstrainRectangle rect; - rect.top_left = tier4_autoware_utils::calcOffsetPose(anchor.pose, max_x, max_y, 0.0).position; - rect.top_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, max_x, min_y, 0.0).position; - rect.bottom_left = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, max_y, 0.0).position; - rect.bottom_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, min_y, 0.0).position; - return rect; -} diff --git a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp new file mode 100644 index 0000000000000..807d113c6b699 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp @@ -0,0 +1,416 @@ +// Copyright 2023 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 "obstacle_avoidance_planner/eb_path_smoother.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/type_alias.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" + +#include +#include +#include + +namespace +{ +Eigen::MatrixXd makePMatrix(const int num_points) +{ + // create P block matrix + Eigen::MatrixXd P_quarter = Eigen::MatrixXd::Zero(num_points, num_points); + for (int r = 0; r < num_points; ++r) { + for (int c = 0; c < num_points; ++c) { + if (r == c) { + if (r == 0 || r == num_points - 1) { + P_quarter(r, c) = 1.0; + } else if (r == 1 || r == num_points - 2) { + P_quarter(r, c) = 5.0; + } else { + P_quarter(r, c) = 6.0; + } + } else if (std::abs(c - r) == 1) { + if (r == 0 || r == num_points - 1) { + P_quarter(r, c) = -2.0; + } else if (c == 0 || c == num_points - 1) { + P_quarter(r, c) = -2.0; + } else { + P_quarter(r, c) = -4.0; + } + } else if (std::abs(c - r) == 2) { + P_quarter(r, c) = 1.0; + } else { + P_quarter(r, c) = 0.0; + } + } + } + + // create P matrix + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(num_points * 2, num_points * 2); + P.block(0, 0, num_points, num_points) = P_quarter; + P.block(num_points, num_points, num_points, num_points) = P_quarter; + + return P; +} + +std::vector toStdVector(const Eigen::VectorXd & eigen_vec) +{ + return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; +} +} // namespace + +namespace obstacle_avoidance_planner +{ +EBPathSmoother::EBParam::EBParam(rclcpp::Node * node) +{ + { // option + enable_warm_start = node->declare_parameter("eb.option.enable_warm_start"); + enable_optimization_validation = + node->declare_parameter("eb.option.enable_optimization_validation"); + } + + { // common + delta_arc_length = node->declare_parameter("eb.common.delta_arc_length"); + num_points = node->declare_parameter("eb.common.num_points"); + } + + { // clearance + num_joint_points = node->declare_parameter("eb.clearance.num_joint_points"); + clearance_for_fix = node->declare_parameter("eb.clearance.clearance_for_fix"); + clearance_for_joint = node->declare_parameter("eb.clearance.clearance_for_joint"); + clearance_for_smooth = node->declare_parameter("eb.clearance.clearance_for_smooth"); + } + + { // weight + smooth_weight = node->declare_parameter("eb.weight.smooth_weight"); + lat_error_weight = node->declare_parameter("eb.weight.lat_error_weight"); + } + + { // qp + qp_param.max_iteration = node->declare_parameter("eb.qp.max_iteration"); + qp_param.eps_abs = node->declare_parameter("eb.qp.eps_abs"); + qp_param.eps_rel = node->declare_parameter("eb.qp.eps_rel"); + } + + // validation + max_validation_error = node->declare_parameter("eb.validation.max_error"); +} + +void EBPathSmoother::EBParam::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + { // option + updateParam(parameters, "eb.option.enable_warm_start", enable_warm_start); + updateParam( + parameters, "eb.option.enable_optimization_validation", enable_optimization_validation); + } + + { // common + updateParam(parameters, "eb.common.delta_arc_length", delta_arc_length); + updateParam(parameters, "eb.common.num_points", num_points); + } + + { // clearance + updateParam(parameters, "eb.clearance.num_joint_points", num_joint_points); + updateParam(parameters, "eb.clearance.clearance_for_fix", clearance_for_fix); + updateParam(parameters, "eb.clearance.clearance_for_joint", clearance_for_joint); + updateParam(parameters, "eb.clearance.clearance_for_smooth", clearance_for_smooth); + } + + { // weight + updateParam(parameters, "eb.weight.smooth_weight", smooth_weight); + updateParam(parameters, "eb.weight.lat_error_weight", lat_error_weight); + } + + { // qp + updateParam(parameters, "eb.qp.max_iteration", qp_param.max_iteration); + updateParam(parameters, "eb.qp.eps_abs", qp_param.eps_abs); + updateParam(parameters, "eb.qp.eps_rel", qp_param.eps_rel); + } +} + +EBPathSmoother::EBPathSmoother( + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const TrajectoryParam & traj_param, const std::shared_ptr time_keeper_ptr) +: enable_debug_info_(enable_debug_info), + ego_nearest_param_(ego_nearest_param), + traj_param_(traj_param), + time_keeper_ptr_(time_keeper_ptr), + logger_(node->get_logger().get_child("eb_path_smoother")) +{ + // eb param + eb_param_ = EBParam(node); + + // publisher + debug_eb_traj_pub_ = node->create_publisher("~/debug/eb_traj", 1); + debug_eb_fixed_traj_pub_ = node->create_publisher("~/debug/eb_fixed_traj", 1); +} + +void EBPathSmoother::onParam(const std::vector & parameters) +{ + eb_param_.onParam(parameters); +} + +void EBPathSmoother::initialize(const bool enable_debug_info, const TrajectoryParam & traj_param) +{ + enable_debug_info_ = enable_debug_info; + traj_param_ = traj_param; +} + +void EBPathSmoother::resetPreviousData() { prev_eb_traj_points_ptr_ = nullptr; } + +std::optional> +EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) +{ + time_keeper_ptr_->tic(__func__); + + const auto & p = planner_data; + + // 1. crop trajectory + const double forward_traj_length = eb_param_.num_points * eb_param_.delta_arc_length; + const double backward_traj_length = traj_param_.output_backward_traj_length; + + const size_t ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const auto cropped_traj_points = trajectory_utils::cropPoints( + p.traj_points, p.ego_pose.position, ego_seg_idx, forward_traj_length, -backward_traj_length); + + // check if goal is contained in cropped_traj_points + const bool is_goal_contained = + geometry_utils::isSamePoint(cropped_traj_points.back(), planner_data.traj_points.back()); + + // 2. insert fixed point + // NOTE: This should be after cropping trajectory so that fixed point will not be cropped. + const auto traj_points_with_fixed_point = insertFixedPoint(cropped_traj_points); + + // 3. resample trajectory with delta_arc_length + const auto resampled_traj_points = [&]() { + // NOTE: If the interval of points is not constant, the optimization is sometimes unstable. + // Therefore, we do not resample a stop point here. + auto tmp_traj_points = trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( + traj_points_with_fixed_point, eb_param_.delta_arc_length); + + // NOTE: The front point is previous optimized one, and the others are the input ones. + // There may be a lateral error between the points, which makes orientation unexpected. + // Therefore, the front pose is updated after resample. + tmp_traj_points.front().pose = traj_points_with_fixed_point.front().pose; + return tmp_traj_points; + }(); + + // 4. pad trajectory points + const auto [padded_traj_points, pad_start_idx] = getPaddedTrajectoryPoints(resampled_traj_points); + + // 5. update constraint for elastic band's QP + updateConstraint(p.header, padded_traj_points, is_goal_contained, pad_start_idx); + + // 6. get optimization result + const auto optimized_points = optimizeTrajectory(); + if (!optimized_points) { + RCLCPP_INFO_EXPRESSION( + logger_, enable_debug_info_, "return std::nullopt since smoothing failed"); + return std::nullopt; + } + + // 7. convert optimization result to trajectory + const auto eb_traj_points = + convertOptimizedPointsToTrajectory(*optimized_points, padded_traj_points, pad_start_idx); + if (!eb_traj_points) { + RCLCPP_WARN(logger_, "return std::nullopt since x or y error is too large"); + return std::nullopt; + } + + prev_eb_traj_points_ptr_ = std::make_shared>(*eb_traj_points); + + // 8. publish eb trajectory + const auto eb_traj = trajectory_utils::createTrajectory(p.header, *eb_traj_points); + debug_eb_traj_pub_->publish(eb_traj); + + time_keeper_ptr_->toc(__func__, " "); + return *eb_traj_points; +} + +std::vector EBPathSmoother::insertFixedPoint( + const std::vector & traj_points) const +{ + time_keeper_ptr_->tic(__func__); + + if (!prev_eb_traj_points_ptr_) { + return traj_points; + } + + auto traj_points_with_fixed_point = traj_points; + // replace the front pose with previous points + trajectory_utils::updateFrontPointForFix( + traj_points_with_fixed_point, *prev_eb_traj_points_ptr_, eb_param_.delta_arc_length, + ego_nearest_param_); + + time_keeper_ptr_->toc(__func__, " "); + return traj_points_with_fixed_point; +} + +std::tuple, size_t> EBPathSmoother::getPaddedTrajectoryPoints( + const std::vector & traj_points) const +{ + time_keeper_ptr_->tic(__func__); + + const size_t pad_start_idx = + std::min(static_cast(eb_param_.num_points), traj_points.size()); + + std::vector padded_traj_points; + for (size_t i = 0; i < static_cast(eb_param_.num_points); ++i) { + const size_t point_idx = i < pad_start_idx ? i : pad_start_idx - 1; + padded_traj_points.push_back(traj_points.at(point_idx)); + } + + time_keeper_ptr_->toc(__func__, " "); + return {padded_traj_points, pad_start_idx}; +} + +void EBPathSmoother::updateConstraint( + const std_msgs::msg::Header & header, const std::vector & traj_points, + const bool is_goal_contained, const int pad_start_idx) +{ + time_keeper_ptr_->tic(__func__); + + const auto & p = eb_param_; + + std::vector debug_fixed_traj_points; // for debug + + const Eigen::MatrixXd A = Eigen::MatrixXd::Identity(p.num_points, p.num_points); + std::vector upper_bound(p.num_points, 0.0); + std::vector lower_bound(p.num_points, 0.0); + for (size_t i = 0; i < static_cast(p.num_points); ++i) { + const double constraint_segment_length = [&]() { + if (i == 0) { + // NOTE: Only first point can be fixed since there is a lateral deviation + // between the two points. + // The front point is previous optimized one, and the others are the input ones. + return p.clearance_for_fix; + } + if (is_goal_contained) { + // NOTE: fix goal and its previous pose to keep goal orientation + if (p.num_points - 2 <= static_cast(i) || pad_start_idx - 2 <= static_cast(i)) { + return p.clearance_for_fix; + } + } + if (i < static_cast(p.num_joint_points) + 1) { // 1 is added since index 0 is fixed + // point + return p.clearance_for_joint; + } + return p.clearance_for_smooth; + }(); + + upper_bound.at(i) = constraint_segment_length; + lower_bound.at(i) = -constraint_segment_length; + + if (constraint_segment_length == 0.0) { + debug_fixed_traj_points.push_back(traj_points.at(i)); + } + } + + Eigen::VectorXd x_mat(2 * p.num_points); + Eigen::MatrixXd theta_mat = Eigen::MatrixXd::Zero(p.num_points, 2 * p.num_points); + for (size_t i = 0; i < static_cast(p.num_points); ++i) { + x_mat(i) = traj_points.at(i).pose.position.x; + x_mat(i + p.num_points) = traj_points.at(i).pose.position.y; + + const double yaw = tf2::getYaw(traj_points.at(i).pose.orientation); + theta_mat(i, i) = -std::sin(yaw); + theta_mat(i, i + p.num_points) = std::cos(yaw); + } + + // calculate P + const Eigen::MatrixXd raw_P_for_smooth = p.smooth_weight * makePMatrix(p.num_points); + const Eigen::MatrixXd P_for_smooth = theta_mat * raw_P_for_smooth * theta_mat.transpose(); + const Eigen::MatrixXd P_for_lat_error = + p.lat_error_weight * Eigen::MatrixXd::Identity(p.num_points, p.num_points); + const Eigen::MatrixXd P = P_for_smooth + P_for_lat_error; + + // calculate q + const Eigen::VectorXd raw_q_for_smooth = theta_mat * raw_P_for_smooth * x_mat; + const auto q = toStdVector(raw_q_for_smooth); + + if (p.enable_warm_start && osqp_solver_ptr_) { + osqp_solver_ptr_->updateP(P); + osqp_solver_ptr_->updateQ(q); + osqp_solver_ptr_->updateA(A); + osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); + osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); + } else { + osqp_solver_ptr_ = std::make_unique( + P, A, q, lower_bound, upper_bound, p.qp_param.eps_abs); + osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); + osqp_solver_ptr_->updateEpsAbs(p.qp_param.eps_abs); + osqp_solver_ptr_->updateMaxIter(p.qp_param.max_iteration); + } + + // publish fixed trajectory + const auto eb_fixed_traj = trajectory_utils::createTrajectory(header, debug_fixed_traj_points); + debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); + + time_keeper_ptr_->toc(__func__, " "); +} + +std::optional> EBPathSmoother::optimizeTrajectory() +{ + time_keeper_ptr_->tic(__func__); + + // solve QP + const auto result = osqp_solver_ptr_->optimize(); + const auto optimized_points = std::get<0>(result); + + const auto status = std::get<3>(result); + + // check status + if (status != 1) { + osqp_solver_ptr_->logUnsolvedStatus("[EB]"); + return std::nullopt; + } + + time_keeper_ptr_->toc(__func__, " "); + return optimized_points; +} + +std::optional> EBPathSmoother::convertOptimizedPointsToTrajectory( + const std::vector & optimized_points, const std::vector & traj_points, + const int pad_start_idx) const +{ + time_keeper_ptr_->tic(__func__); + + std::vector eb_traj_points; + + // update only x and y + for (size_t i = 0; i < static_cast(pad_start_idx); ++i) { + const double lat_offset = optimized_points.at(i); + + // validate optimization result + if (eb_param_.enable_optimization_validation) { + if (eb_param_.max_validation_error < std::abs(lat_offset)) { + return std::nullopt; + } + } + + auto eb_traj_point = traj_points.at(i); + eb_traj_point.pose = + tier4_autoware_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); + eb_traj_points.push_back(eb_traj_point); + } + + // update orientation + motion_utils::insertOrientation(eb_traj_points, true); + + time_keeper_ptr_->toc(__func__, " "); + return eb_traj_points; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index bf62a10bbf8a5..dc95d7e1809a0 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -15,882 +15,948 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" - -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/assign/list_of.hpp" -#include "boost/optional.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include #include #include -#include +#include #include -#include +namespace obstacle_avoidance_planner +{ namespace { -geometry_msgs::msg::Pose convertRefPointsToPose(const ReferencePoint & ref_point) +std::tuple, std::vector> calcVehicleCirclesByUniformCircle( + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const double radius_ratio) { - geometry_msgs::msg::Pose pose; - pose.position = ref_point.p; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); - return pose; + const double radius = std::hypot( + vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, + vehicle_info.vehicle_width_m / 2.0) * + radius_ratio; + const std::vector radiuses(circle_num, radius); + + const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); + std::vector longitudinal_offsets; + for (size_t i = 0; i < circle_num; ++i) { + longitudinal_offsets.push_back( + unit_lon_length / 2.0 + unit_lon_length * i - vehicle_info.rear_overhang_m); + } + + return {radiuses, longitudinal_offsets}; } -std::tuple extractBounds( - const std::vector & ref_points, const size_t l_idx, - const VehicleParam & vehicle_param_) +std::tuple, std::vector> calcVehicleCirclesByBicycleModel( + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const double rear_radius_ratio, const double front_radius_ratio) { - const double base_to_right = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.right_overhang; - const double base_to_left = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.left_overhang; - Eigen::VectorXd ub_vec(ref_points.size()); - Eigen::VectorXd lb_vec(ref_points.size()); - for (size_t i = 0; i < ref_points.size(); ++i) { - ub_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).upper_bound + base_to_left; - lb_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).lower_bound - base_to_right; + if (circle_num < 2) { + throw std::invalid_argument("circle_num is less than 2."); } - return {ub_vec, lb_vec}; + + // 1st circle (rear wheel) + const double rear_radius = vehicle_info.vehicle_width_m / 2.0 * rear_radius_ratio; + const double rear_lon_offset = 0.0; + + // 2nd circle (front wheel) + const double front_radius = + std::hypot( + vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, + vehicle_info.vehicle_width_m / 2.0) * + front_radius_ratio; + + const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); + const double front_lon_offset = + unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_info.rear_overhang_m; + + return {{rear_radius, front_radius}, {rear_lon_offset, front_lon_offset}}; } -/* -Bounds findWidestBounds(const BoundsCandidates & front_bounds_candidates) +std::tuple, std::vector> calcVehicleCirclesByFittingUniformCircle( + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num) { - double max_width = std::numeric_limits::min(); - size_t max_width_index = 0; - if (front_bounds_candidates.size() != 1) { - for (size_t candidate_idx = 0; candidate_idx < front_bounds_candidates.size(); - ++candidate_idx) { - const auto & front_bounds_candidate = front_bounds_candidates.at(candidate_idx); - const double bound_width = - front_bounds_candidate.upper_bound - front_bounds_candidate.lower_bound; - if (max_width < bound_width) { - max_width_index = candidate_idx; - max_width = bound_width; - } - } + if (circle_num < 2) { + throw std::invalid_argument("circle_num is less than 2."); } - return front_bounds_candidates.at(max_width_index); -} -*/ -geometry_msgs::msg::Pose calcVehiclePose( - const ReferencePoint & ref_point, const double lat_error, const double yaw_error, - const double offset) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = ref_point.p.x - std::sin(ref_point.yaw) * lat_error - - std::cos(ref_point.yaw + yaw_error) * offset; - pose.position.y = ref_point.p.y + std::cos(ref_point.yaw) * lat_error - - std::sin(ref_point.yaw + yaw_error) * offset; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + yaw_error); - - return pose; + const double radius = vehicle_info.vehicle_width_m / 2.0; + std::vector radiuses(circle_num, radius); + + const double unit_lon_length = + vehicle_info.vehicle_length_m / static_cast(circle_num - 1); + std::vector longitudinal_offsets; + for (size_t i = 0; i < circle_num; ++i) { + longitudinal_offsets.push_back(unit_lon_length * i - vehicle_info.rear_overhang_m); + radiuses.push_back(radius); + } + + return {radiuses, longitudinal_offsets}; } -template -void trimPoints(std::vector & points) +std::tuple extractBounds( + const std::vector & ref_points, const size_t l_idx, const double offset) { - std::vector trimmed_points; - constexpr double epsilon = 1e-6; - - auto itr = points.begin(); - while (itr != points.end() - 1) { - bool is_overlapping = false; - if (itr != points.begin()) { - const auto & p_front = tier4_autoware_utils::getPoint(*itr); - const auto & p_back = tier4_autoware_utils::getPoint(*(itr + 1)); - - const double dx = p_front.x - p_back.x; - const double dy = p_front.y - p_back.y; - if (dx * dx + dy * dy < epsilon) { - is_overlapping = true; - } - } - if (is_overlapping) { - itr = points.erase(itr); - } else { - ++itr; - } + Eigen::VectorXd ub_vec(ref_points.size()); + Eigen::VectorXd lb_vec(ref_points.size()); + for (size_t i = 0; i < ref_points.size(); ++i) { + ub_vec(i) = ref_points.at(i).bounds_on_constraints.at(l_idx).upper_bound + offset; + lb_vec(i) = ref_points.at(i).bounds_on_constraints.at(l_idx).lower_bound - offset; } + return {ub_vec, lb_vec}; } -std::vector eigenVectorToStdVector(const Eigen::VectorXd & eigen_vec) +std::vector toStdVector(const Eigen::VectorXd & eigen_vec) { return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; } -double calcLateralError( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose ref_pose) +// NOTE: much faster than boost::geometry::intersection() +std::optional intersect( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) { - const double err_x = target_point.x - ref_pose.position.x; - const double err_y = target_point.y - ref_pose.position.y; - const double ref_yaw = tf2::getYaw(ref_pose.orientation); - const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; - return lat_err; + // calculate intersection point + const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); + if (det == 0.0) { + return std::nullopt; + } + + const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; + const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; + if (t < 0 || 1 < t || s < 0 || 1 < s) { + return std::nullopt; + } + + geometry_msgs::msg::Point intersect_point; + intersect_point.x = t * p1.x + (1.0 - t) * p2.x; + intersect_point.y = t * p1.y + (1.0 - t) * p2.y; + return intersect_point; } -Eigen::Vector2d getState( - const geometry_msgs::msg::Pose & target_pose, const geometry_msgs::msg::Pose & ref_pose) +bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { - const double lat_error = calcLateralError(target_pose.position, ref_pose); - const double yaw_error = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(target_pose.orientation) - tf2::getYaw(ref_pose.orientation)); - Eigen::VectorXd kinematics = Eigen::VectorXd::Zero(2); - kinematics << lat_error, yaw_error; - return kinematics; + const double base_theta = tf2::getYaw(pose.orientation); + const double target_theta = tier4_autoware_utils::calcAzimuthAngle(pose.position, target_pos); + const double diff_theta = tier4_autoware_utils::normalizeRadian(target_theta - base_theta); + return diff_theta > 0; } -std::vector splineYawFromReferencePoints(const std::vector & ref_points) +// NOTE: Regarding boundary's sign, left is positive, and right is negative +double calcLateralDistToBounds( + const geometry_msgs::msg::Pose & pose, const std::vector & bound, + const double additional_offset, const bool is_left_bound = true) { - if (ref_points.size() == 1) { - return {ref_points.front().yaw}; - } + constexpr double max_lat_offset_for_left = 5.0; + constexpr double min_lat_offset_for_left = -5.0; - std::vector points; - for (const auto & ref_point : ref_points) { - points.push_back(ref_point.p); + const double max_lat_offset = is_left_bound ? max_lat_offset_for_left : -max_lat_offset_for_left; + const double min_lat_offset = is_left_bound ? min_lat_offset_for_left : -min_lat_offset_for_left; + const auto max_lat_offset_point = + tier4_autoware_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; + const auto min_lat_offset_point = + tier4_autoware_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; + + double closest_dist_to_bound = max_lat_offset; + for (size_t i = 0; i < bound.size() - 1; ++i) { + const auto intersect_point = + intersect(min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); + if (intersect_point) { + const bool is_point_left = isLeft(pose, *intersect_point); + const double dist_to_bound = + tier4_autoware_utils::calcDistance2d(pose.position, *intersect_point) * + (is_point_left ? 1.0 : -1.0); + + closest_dist_to_bound = + is_left_bound ? std::min(dist_to_bound - additional_offset, closest_dist_to_bound) + : std::max(dist_to_bound + additional_offset, closest_dist_to_bound); + } } - return interpolation::splineYawFromPoints(points); + + return closest_dist_to_bound; } +} // namespace -size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) +MPTOptimizer::MPTParam::MPTParam( + rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info) { - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + { // option + steer_limit_constraint = node->declare_parameter("mpt.option.steer_limit_constraint"); + enable_warm_start = node->declare_parameter("mpt.option.enable_warm_start"); + enable_manual_warm_start = node->declare_parameter("mpt.option.enable_manual_warm_start"); + enable_optimization_validation = + node->declare_parameter("mpt.option.enable_optimization_validation"); + mpt_visualize_sampling_num = node->declare_parameter("mpt.option.visualize_sampling_num"); + } - const auto nearest_idx_optional = - motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points_with_yaw, pose.position); -} + { // common + num_points = node->declare_parameter("mpt.common.num_points"); + delta_arc_length = node->declare_parameter("mpt.common.delta_arc_length"); + } -boost::optional intersection( - const Eigen::Vector2d & start_point1, const Eigen::Vector2d & end_point1, - const Eigen::Vector2d & start_point2, const Eigen::Vector2d & end_point2) -{ - using Point = boost::geometry::model::d2::point_xy; - using Line = boost::geometry::model::linestring; + // kinematics + max_steer_rad = vehicle_info.max_steer_angle_rad; + + // NOTE: By default, optimization_center_offset will be vehicle_info.wheel_base * 0.8 + // The 0.8 scale is adopted as it performed the best. + constexpr double default_wheel_base_ratio = 0.8; + optimization_center_offset = node->declare_parameter( + "mpt.kinematics.optimization_center_offset", + vehicle_info.wheel_base_m * default_wheel_base_ratio); + + { // clearance + hard_clearance_from_road = + node->declare_parameter("mpt.clearance.hard_clearance_from_road"); + soft_clearance_from_road = + node->declare_parameter("mpt.clearance.soft_clearance_from_road"); + } - const Line line1 = - boost::assign::list_of(start_point1(0), start_point1(1))(end_point1(0), end_point1(1)); - const Line line2 = - boost::assign::list_of(start_point2(0), start_point2(1))(end_point2(0), end_point2(1)); + { // weight + soft_collision_free_weight = + node->declare_parameter("mpt.weight.soft_collision_free_weight"); + + lat_error_weight = node->declare_parameter("mpt.weight.lat_error_weight"); + yaw_error_weight = node->declare_parameter("mpt.weight.yaw_error_weight"); + yaw_error_rate_weight = node->declare_parameter("mpt.weight.yaw_error_rate_weight"); + steer_input_weight = node->declare_parameter("mpt.weight.steer_input_weight"); + steer_rate_weight = node->declare_parameter("mpt.weight.steer_rate_weight"); + + terminal_lat_error_weight = + node->declare_parameter("mpt.weight.terminal_lat_error_weight"); + terminal_yaw_error_weight = + node->declare_parameter("mpt.weight.terminal_yaw_error_weight"); + goal_lat_error_weight = node->declare_parameter("mpt.weight.goal_lat_error_weight"); + goal_yaw_error_weight = node->declare_parameter("mpt.weight.goal_yaw_error_weight"); + } - std::vector output; - boost::geometry::intersection(line1, line2, output); - if (output.empty()) { - return {}; + { // avoidance + max_avoidance_cost = node->declare_parameter("mpt.avoidance.max_avoidance_cost"); + avoidance_cost_margin = node->declare_parameter("mpt.avoidance.avoidance_cost_margin"); + avoidance_cost_band_length = + node->declare_parameter("mpt.avoidance.avoidance_cost_band_length"); + avoidance_cost_decrease_rate = + node->declare_parameter("mpt.avoidance.avoidance_cost_decrease_rate"); + + avoidance_lat_error_weight = + node->declare_parameter("mpt.avoidance.weight.lat_error_weight"); + avoidance_yaw_error_weight = + node->declare_parameter("mpt.avoidance.weight.yaw_error_weight"); + avoidance_steer_input_weight = + node->declare_parameter("mpt.avoidance.weight.steer_input_weight"); + } + + { // collision free constraints + l_inf_norm = node->declare_parameter("mpt.collision_free_constraints.option.l_inf_norm"); + soft_constraint = + node->declare_parameter("mpt.collision_free_constraints.option.soft_constraint"); + hard_constraint = + node->declare_parameter("mpt.collision_free_constraints.option.hard_constraint"); + } + + { // vehicle_circles + // NOTE: Vehicle shape for collision free constraints is considered as a set of circles + vehicle_circles_method = + node->declare_parameter("mpt.collision_free_constraints.vehicle_circles.method"); + + // uniform circles + vehicle_circles_uniform_circle_num = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.uniform_circle.num"); + vehicle_circles_uniform_circle_radius_ratio = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio"); + + // bicycle model + vehicle_circles_bicycle_model_num = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.bicycle_model.num_for_" + "calculation"); + vehicle_circles_bicycle_model_rear_radius_ratio = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles." + "bicycle_model.rear_radius_ratio"); + vehicle_circles_bicycle_model_front_radius_ratio = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles." + "bicycle_model.front_radius_ratio"); + + // fitting uniform circles + vehicle_circles_fitting_uniform_circle_num = node->declare_parameter( + "mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num"); } - const Eigen::Vector2d output_point{output.front().x(), output.front().y()}; - return output_point; + { // validation + max_validation_lat_error = node->declare_parameter("mpt.validation.max_lat_error"); + max_validation_yaw_error = node->declare_parameter("mpt.validation.max_yaw_error"); + } } -double calcLateralDistToBound( - const Eigen::Vector2d & current_point, const Eigen::Vector2d & edge_point, - const std::vector & bound, const bool is_right_bound = false) +void MPTOptimizer::MPTParam::onParam(const std::vector & parameters) { - for (size_t i = 0; i < bound.size() - 1; ++i) { - const Eigen::Vector2d current_bound_point = {bound.at(i).x, bound.at(i).y}; - const Eigen::Vector2d next_bound_point = {bound.at(i + 1).x, bound.at(i + 1).y}; - - const auto intersects_point = - intersection(current_point, edge_point, current_bound_point, next_bound_point); - if (intersects_point) { - const double dist = (*intersects_point - current_point).norm(); - return is_right_bound ? -dist : dist; - } + using tier4_autoware_utils::updateParam; + + { // option + updateParam(parameters, "mpt.option.steer_limit_constraint", steer_limit_constraint); + updateParam(parameters, "mpt.option.enable_warm_start", enable_warm_start); + updateParam(parameters, "mpt.option.enable_manual_warm_start", enable_manual_warm_start); + updateParam( + parameters, "mpt.option.enable_optimization_validation", enable_optimization_validation); + updateParam(parameters, "mpt.option.visualize_sampling_num", mpt_visualize_sampling_num); + } + + // common + updateParam(parameters, "mpt.common.num_points", num_points); + updateParam(parameters, "mpt.common.delta_arc_length", delta_arc_length); + + // kinematics + updateParam( + parameters, "mpt.kinematics.optimization_center_offset", optimization_center_offset); + + // collision_free_constraints + updateParam(parameters, "mpt.collision_free_constraints.option.l_inf_norm", l_inf_norm); + updateParam( + parameters, "mpt.collision_free_constraints.option.soft_constraint", soft_constraint); + updateParam( + parameters, "mpt.collision_free_constraints.option.hard_constraint", hard_constraint); + + { // vehicle_circles + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.method", vehicle_circles_method); + + // uniform circles + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.uniform_circle.num", + vehicle_circles_uniform_circle_num); + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio", + vehicle_circles_uniform_circle_radius_ratio); + + // bicycle model + updateParam( + parameters, + "mpt.collision_free_constraints.vehicle_circles.bicycle_model.num_for_calculation", + vehicle_circles_bicycle_model_num); + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.bicycle_model.rear_radius_ratio", + vehicle_circles_bicycle_model_rear_radius_ratio); + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.bicycle_model.front_radius_ratio", + vehicle_circles_bicycle_model_front_radius_ratio); + + // fitting uniform circles + updateParam( + parameters, "mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num", + vehicle_circles_fitting_uniform_circle_num); + } + + { // clearance + updateParam( + parameters, "mpt.clearance.hard_clearance_from_road", hard_clearance_from_road); + updateParam( + parameters, "mpt.clearance.soft_clearance_from_road", soft_clearance_from_road); } - return is_right_bound ? -5.0 : 5.0; + { // weight + updateParam( + parameters, "mpt.weight.soft_collision_free_weight", soft_collision_free_weight); + + updateParam(parameters, "mpt.weight.lat_error_weight", lat_error_weight); + updateParam(parameters, "mpt.weight.yaw_error_weight", yaw_error_weight); + updateParam(parameters, "mpt.weight.yaw_error_rate_weight", yaw_error_rate_weight); + updateParam(parameters, "mpt.weight.steer_input_weight", steer_input_weight); + updateParam(parameters, "mpt.weight.steer_rate_weight", steer_rate_weight); + + updateParam( + parameters, "mpt.weight.terminal_lat_error_weight", terminal_lat_error_weight); + updateParam( + parameters, "mpt.weight.terminal_yaw_error_weight", terminal_yaw_error_weight); + updateParam(parameters, "mpt.weight.goal_lat_error_weight", goal_lat_error_weight); + updateParam(parameters, "mpt.weight.goal_yaw_error_weight", goal_yaw_error_weight); + } + + { // avoidance + updateParam(parameters, "mpt.avoidance.max_avoidance_cost", max_avoidance_cost); + updateParam(parameters, "mpt.avoidance.avoidance_cost_margin", avoidance_cost_margin); + updateParam( + parameters, "mpt.avoidance.avoidance_cost_band_length", avoidance_cost_band_length); + updateParam( + parameters, "mpt.avoidance.avoidance_cost_decrease_rate", avoidance_cost_decrease_rate); + + updateParam( + parameters, "mpt.avoidance.weight.lat_error_weight", avoidance_lat_error_weight); + updateParam( + parameters, "mpt.avoidance.weight.yaw_error_weight", avoidance_yaw_error_weight); + updateParam( + parameters, "mpt.avoidance.weight.steer_input_weight", avoidance_steer_input_weight); + } + + { // validation + updateParam(parameters, "mpt.validation.max_lat_error", max_validation_lat_error); + updateParam(parameters, "mpt.validation.max_yaw_error", max_validation_yaw_error); + } } -} // namespace MPTOptimizer::MPTOptimizer( - const bool is_showing_debug_info, const TrajectoryParam & traj_param, - const VehicleParam & vehicle_param, const MPTParam & mpt_param) -: is_showing_debug_info_(is_showing_debug_info), + rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, + const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, + const std::shared_ptr debug_data_ptr, + const std::shared_ptr time_keeper_ptr) +: enable_debug_info_(enable_debug_info), + ego_nearest_param_(ego_nearest_param), + vehicle_info_(vehicle_info), traj_param_(traj_param), - vehicle_param_(vehicle_param), - mpt_param_(mpt_param), - vehicle_model_ptr_( - std::make_unique(vehicle_param_.wheelbase, mpt_param_.max_steer_rad)), - osqp_solver_ptr_(std::make_unique(osqp_epsilon_)) + debug_data_ptr_(debug_data_ptr), + time_keeper_ptr_(time_keeper_ptr), + logger_(node->get_logger().get_child("mpt_optimizer")) { + // initialize mpt param + mpt_param_ = MPTParam(node, vehicle_info); + updateVehicleCircles(); + debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; + + // state equation generator + state_equation_generator_ = + StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_ptr_); + + // osqp solver + osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); + + // publisher + debug_fixed_traj_pub_ = node->create_publisher("~/debug/mpt_fixed_traj", 1); + debug_ref_traj_pub_ = node->create_publisher("~/debug/mpt_ref_traj", 1); + debug_mpt_traj_pub_ = node->create_publisher("~/debug/mpt_traj", 1); } -boost::optional MPTOptimizer::getModelPredictiveTrajectory( - const bool enable_avoidance, - const std::vector & smoothed_points, - const std::vector & path_points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_trajs, - const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, - DebugData & debug_data) +void MPTOptimizer::updateVehicleCircles() { - stop_watch_.tic(__func__); - - current_ego_pose_ = current_ego_pose; - current_ego_vel_ = current_ego_vel; - - if (smoothed_points.empty()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since smoothed_points is empty"); - return boost::none; + const auto & p = mpt_param_; + + if (p.vehicle_circles_method == "uniform_circle") { + std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = + calcVehicleCirclesByUniformCircle( + vehicle_info_, p.vehicle_circles_uniform_circle_num, + p.vehicle_circles_uniform_circle_radius_ratio); + } else if (p.vehicle_circles_method == "bicycle_model") { + std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = + calcVehicleCirclesByBicycleModel( + vehicle_info_, p.vehicle_circles_bicycle_model_num, + p.vehicle_circles_bicycle_model_front_radius_ratio, + p.vehicle_circles_bicycle_model_rear_radius_ratio); + } else if (p.vehicle_circles_method == "fitting_uniform_circle") { + std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = + calcVehicleCirclesByFittingUniformCircle( + vehicle_info_, p.vehicle_circles_fitting_uniform_circle_num); + } else { + throw std::invalid_argument("mpt_param_.vehicle_circles_method is invalid."); } - std::vector full_ref_points = - getReferencePoints(smoothed_points, left_bound, right_bound, prev_trajs, debug_data); - if (full_ref_points.empty()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since ref_points is empty"); - return boost::none; - } else if (full_ref_points.size() == 1) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since ref_points.size() == 1"); - return boost::none; - } + debug_data_ptr_->vehicle_circle_radiuses = vehicle_circle_radiuses_; + debug_data_ptr_->vehicle_circle_longitudinal_offsets = vehicle_circle_longitudinal_offsets_; +} - debug_data.mpt_fixed_traj = getMPTFixedPoints(full_ref_points); +void MPTOptimizer::initialize(const bool enable_debug_info, const TrajectoryParam & traj_param) +{ + enable_debug_info_ = enable_debug_info; + traj_param_ = traj_param; +} - std::vector fixed_ref_points; - std::vector non_fixed_ref_points; - bool is_fixing_ref_points = true; - for (size_t i = 0; i < full_ref_points.size(); ++i) { - if (i == full_ref_points.size() - 1) { - if (!full_ref_points.at(i).fix_kinematic_state) { - is_fixing_ref_points = false; - } - } else if ( - // fix first three points - full_ref_points.at(i).fix_kinematic_state && full_ref_points.at(i + 1).fix_kinematic_state && - (i + 2 < full_ref_points.size() && full_ref_points.at(i + 2).fix_kinematic_state) && - (i + 3 < full_ref_points.size() && full_ref_points.at(i + 3).fix_kinematic_state)) { - } else { - is_fixing_ref_points = false; - } +void MPTOptimizer::resetPreviousData() { prev_ref_points_ptr_ = nullptr; } - if (is_fixing_ref_points) { - fixed_ref_points.push_back(full_ref_points.at(i)); - } else { - non_fixed_ref_points.push_back(full_ref_points.at(i)); - } - } +void MPTOptimizer::onParam(const std::vector & parameters) +{ + mpt_param_.onParam(parameters); + updateVehicleCircles(); + debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; +} - // calculate B and W matrices - const auto mpt_matrix = generateMPTMatrix(non_fixed_ref_points, debug_data); +std::optional> MPTOptimizer::getModelPredictiveTrajectory( + const PlannerData & planner_data, const std::vector & smoothed_points) +{ + time_keeper_ptr_->tic(__func__); - // calculate Q and R matrices - const auto val_matrix = generateValueMatrix(non_fixed_ref_points, path_points, debug_data); + const auto & p = planner_data; + const auto & traj_points = p.traj_points; - const auto optimized_control_variables = executeOptimization( - prev_trajs, enable_avoidance, mpt_matrix, val_matrix, non_fixed_ref_points, debug_data); - if (!optimized_control_variables) { + // 1. calculate reference points + auto ref_points = calcReferencePoints(planner_data, smoothed_points); + if (ref_points.size() < 2) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "return boost::none since could not solve qp"); - return boost::none; + logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2."); + return std::nullopt; } - const auto mpt_points = getMPTPoints( - fixed_ref_points, non_fixed_ref_points, optimized_control_variables.get(), mpt_matrix, - debug_data); - - // NOTE: Sometimes optimization failed without failed status. - // Therefore, we have to check if optimization was solved correctly by the result. - constexpr double max_lateral_deviation = 3.0; - for (const double lateral_error : debug_data.lateral_errors) { - if (max_lateral_deviation < std::abs(lateral_error)) { - RCLCPP_ERROR( - rclcpp::get_logger("mpt_optimizer"), - "return boost::none since lateral deviation is too large."); - return boost::none; - } - } - constexpr double max_yaw_deviation = 50.0 / 180 * 3.14; - for (const double yaw_error : debug_data.yaw_errors) { - if (max_yaw_deviation < std::abs(yaw_error)) { - RCLCPP_ERROR( - rclcpp::get_logger("mpt_optimizer"), - "return boost::none since yaw deviation is too large."); - return boost::none; - } - } + // 2. calculate B and W matrices where x = B u + W + const auto mpt_mat = state_equation_generator_.calcMatrix(ref_points); - auto full_optimized_ref_points = fixed_ref_points; - full_optimized_ref_points.insert( - full_optimized_ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); + // 3. calculate Q and R matrices where J(x, u) = x^t Q x + u^t R u + const auto val_mat = calcValueMatrix(ref_points, traj_points); - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + // 4. get objective matrix + const auto obj_mat = calcObjectiveMatrix(mpt_mat, val_mat, ref_points); - MPTTrajs mpt_trajs; - mpt_trajs.mpt = mpt_points; - mpt_trajs.ref_points = full_optimized_ref_points; - return mpt_trajs; -} + // 5. get constraints matrix + const auto const_mat = calcConstraintMatrix(mpt_mat, ref_points); -std::vector MPTOptimizer::getReferencePoints( - const std::vector & smoothed_points, - const std::vector & left_bound, - const std::vector & right_bound, - const std::unique_ptr & prev_trajs, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - const auto ref_points = [&]() { - auto ref_points = [&]() { - // TODO(murooka) consider better algorithm to calculate fixed/non-fixed reference points - const auto fixed_ref_points = getFixedReferencePoints(smoothed_points, prev_trajs); - - // if no fixed_ref_points - if (fixed_ref_points.empty()) { - // interpolate and crop backward - const auto interpolated_points = interpolation_utils::getInterpolatedPoints( - smoothed_points, mpt_param_.delta_arc_length_for_mpt_points); - const auto interpolated_points_with_yaw = - points_utils::convertToPosesWithYawEstimation(interpolated_points); - const auto cropped_interpolated_points_with_yaw = points_utils::clipBackwardPoints( - interpolated_points_with_yaw, current_ego_pose_, traj_param_.backward_fixing_distance, - mpt_param_.delta_arc_length_for_mpt_points, - traj_param_.delta_yaw_threshold_for_closest_point); - const auto cropped_interpolated_points = - points_utils::convertToPoints(cropped_interpolated_points_with_yaw); - - return points_utils::convertToReferencePoints(cropped_interpolated_points); - } + // 6. optimize steer angles + const auto optimized_steer_angles = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); + if (!optimized_steer_angles) { + RCLCPP_INFO_EXPRESSION( + logger_, enable_debug_info_, "return std::nullopt since could not solve qp"); + return std::nullopt; + } - // calc non fixed traj points - // const auto fixed_ref_points_with_yaw = points_utils::convertToPosesWithYawEstimation( - // points_utils::convertToPoints(fixed_ref_points)); - const auto fixed_ref_points_with_yaw = points_utils::convertToPoses(fixed_ref_points); - const auto seg_idx_optional = motion_utils::findNearestSegmentIndex( - smoothed_points, fixed_ref_points_with_yaw.back(), std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - - if (!seg_idx_optional) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "cannot find nearest segment index in getReferencePoints"); - return std::vector{}; - } - const auto seg_idx = *seg_idx_optional; - const auto non_fixed_traj_points = - std::vector{ - smoothed_points.begin() + seg_idx, smoothed_points.end()}; - - const double offset = motion_utils::calcLongitudinalOffsetToSegment( - non_fixed_traj_points, 0, fixed_ref_points.back().p) + - mpt_param_.delta_arc_length_for_mpt_points; - const auto non_fixed_interpolated_traj_points = interpolation_utils::getInterpolatedPoints( - non_fixed_traj_points, mpt_param_.delta_arc_length_for_mpt_points, offset); - const auto non_fixed_ref_points = - points_utils::convertToReferencePoints(non_fixed_interpolated_traj_points); - - // make ref points - auto ref_points = fixed_ref_points; - ref_points.insert(ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); - - return ref_points; - }(); + // 7. convert to points with validation + const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_steer_angles, mpt_mat); + if (!mpt_traj_points) { + RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large."); + return std::nullopt; + } - if (ref_points.empty()) { - return std::vector{}; - } + // 8. publish trajectories for debug + publishDebugTrajectories(p.header, ref_points, *mpt_traj_points); - // set some information to reference points considering fix kinematics - trimPoints(ref_points); - calcOrientation(ref_points); - calcCurvature(ref_points); - calcArcLength(ref_points); - calcPlanningFromEgo( - ref_points); // NOTE: fix_kinematic_state will be updated when planning from ego - - // crop trajectory with margin to calculate vehicle bounds at the end point - constexpr double tmp_ref_points_margin = 20.0; - const double ref_length_with_margin = - traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points + - tmp_ref_points_margin; - ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length_with_margin); - if (ref_points.empty()) { - return std::vector{}; - } + time_keeper_ptr_->toc(__func__, " "); + + debug_data_ptr_->ref_points = ref_points; + prev_ref_points_ptr_ = std::make_shared>(ref_points); - // set bounds information - calcBounds(ref_points, left_bound, right_bound, debug_data); - calcVehicleBounds(ref_points, debug_data); + return *mpt_traj_points; +} - // set extra information (alpha and has_object_collision) - // NOTE: This must be after bounds calculation. - calcExtraPoints(ref_points, prev_trajs); +std::vector MPTOptimizer::calcReferencePoints( + const PlannerData & planner_data, const std::vector & smoothed_points) const +{ + time_keeper_ptr_->tic(__func__); - const double ref_length = - traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; - ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length); + const auto & p = planner_data; - // bounds information is assigned to debug data after truncating reference points - debug_data.ref_points = ref_points; + const double forward_traj_length = mpt_param_.num_points * mpt_param_.delta_arc_length; + const double backward_traj_length = traj_param_.output_backward_traj_length; - return ref_points; + // 1. resample and convert smoothed points type from trajectory points to reference points + time_keeper_ptr_->tic("resampleReferencePoints"); + auto ref_points = [&]() { + const auto resampled_smoothed_points = + trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( + smoothed_points, mpt_param_.delta_arc_length); + return trajectory_utils::convertToReferencePoints(resampled_smoothed_points); }(); - if (ref_points.empty()) { - return std::vector{}; + time_keeper_ptr_->toc("resampleReferencePoints", " "); + + // 2. crop forward and backward with margin, and calculate spline interpolation + // NOTE: Margin is added to calculate orientation, curvature, etc precisely. + // Start point may change. Spline calculation is required. + constexpr double tmp_margin = 10.0; + size_t ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); + ref_points = trajectory_utils::cropPoints( + ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, + -backward_traj_length - tmp_margin); + SplineInterpolationPoints2d ref_points_spline(ref_points); + ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); + + // 3. calculate orientation and curvature + updateOrientation(ref_points, ref_points_spline); + updateCurvature(ref_points, ref_points_spline); + + // 4. crop backward + // NOTE: Start point may change. Spline calculation is required. + ref_points = trajectory_utils::cropPoints( + ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, + -backward_traj_length); + ref_points_spline = SplineInterpolationPoints2d(ref_points); + ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); + + // 5. update fixed points, and resample + // NOTE: This must be after backward cropping. + // New start point may be added and resampled. Spline calculation is required. + updateFixedPoint(ref_points); + ref_points_spline = SplineInterpolationPoints2d(ref_points); + + // 6. update bounds + // NOTE: After this, resample must not be called since bounds are not interpolated. + updateBounds(ref_points, p.left_bound, p.right_bound); + updateVehicleBounds(ref_points, ref_points_spline); + + // 7. update delta arc length + updateDeltaArcLength(ref_points); + + // 8. update extra information (alpha and beta) + // NOTE: This must be after calculation of bounds and delta arc length + updateExtraPoints(ref_points); + + // 9. crop forward + // ref_points = trajectory_utils::cropForwardPoints( + // ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length); + if (static_cast(mpt_param_.num_points) < ref_points.size()) { + ref_points.resize(mpt_param_.num_points); } - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); + return ref_points; } -void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) const +void MPTOptimizer::updateOrientation( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const { - // if plan from ego - constexpr double epsilon = 1e-04; - const double trajectory_length = motion_utils::calcArcLength(ref_points); - - const bool plan_from_ego = mpt_param_.plan_from_ego && std::abs(current_ego_vel_) < epsilon && - ref_points.size() > 1 && - trajectory_length < mpt_param_.max_plan_from_ego_length; - if (plan_from_ego) { - for (auto & ref_point : ref_points) { - ref_point.fix_kinematic_state = boost::none; - } - - /* - // interpolate and crop backward - const auto interpolated_points = interpolation_utils::getInterpolatedPoints( - points, - mpt_param_.delta_arc_length_for_mpt_points); const auto cropped_interpolated_points = - points_utils::clipBackwardPoints( interpolated_points, current_pose_.position, - traj_param_.backward_fixing_distance, mpt_param_.delta_arc_length_for_mpt_points); - - auto cropped_ref_points = - points_utils::convertToReferencePoints(cropped_interpolated_points); - */ - - // assign fix kinematics - const size_t nearest_ref_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)), - current_ego_pose_, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold); - - // calculate cropped_ref_points.at(nearest_ref_idx) with yaw - const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose { - geometry_msgs::msg::Pose pose; - pose.position = ref_points.at(nearest_ref_idx).p; - - const size_t prev_nearest_ref_idx = - (nearest_ref_idx == ref_points.size() - 1) ? nearest_ref_idx - 1 : nearest_ref_idx; - pose.orientation = geometry_utils::getQuaternionFromPoints( - ref_points.at(prev_nearest_ref_idx + 1).p, ref_points.at(prev_nearest_ref_idx).p); - return pose; - }(); + const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); + for (size_t i = 0; i < ref_points.size(); ++i) { + ref_points.at(i).pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i)); + } +} - ref_points.at(nearest_ref_idx).fix_kinematic_state = - getState(current_ego_pose_, nearest_ref_pose); - ref_points.at(nearest_ref_idx).plan_from_ego = true; +void MPTOptimizer::updateCurvature( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const +{ + const auto curvature_vec = ref_points_spline.getSplineInterpolatedCurvatures(); + for (size_t i = 0; i < ref_points.size(); ++i) { + ref_points.at(i).curvature = curvature_vec.at(i); } } -std::vector MPTOptimizer::getFixedReferencePoints( - const std::vector & points, - const std::unique_ptr & prev_trajs) const +void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - if ( - !prev_trajs || prev_trajs->model_predictive_trajectory.empty() || - prev_trajs->mpt_ref_points.empty() || - prev_trajs->model_predictive_trajectory.size() != prev_trajs->mpt_ref_points.size()) { - return std::vector(); + time_keeper_ptr_->tic(__func__); + + if (!prev_ref_points_ptr_) { + // no fixed point + return; } - if (!mpt_param_.fix_points_around_ego) { - return std::vector(); + // replace the front pose and curvature with previous reference points + const auto idx = trajectory_utils::updateFrontPointForFix( + ref_points, *prev_ref_points_ptr_, mpt_param_.delta_arc_length, ego_nearest_param_); + + // NOTE: memorize front point to be fixed before resampling + const auto front_point = ref_points.front(); + + if (idx && *idx != 0) { + // In order to fix the front "orientation" defined by two front points, insert the previous + // fixed point. + ref_points.insert(ref_points.begin(), prev_ref_points_ptr_->at(static_cast(*idx) - 1)); + + // resample to make ref_points' interval constant. + // NOTE: Only pose, velocity and curvature will be interpolated. + ref_points = trajectory_utils::resampleReferencePoints(ref_points, mpt_param_.delta_arc_length); + + // update pose which is previous one, and fixed kinematic state + // NOTE: There may be a lateral error between the previous and input points. + // Therefore, the pose for fix should not be resampled. + const auto & prev_ref_front_point = prev_ref_points_ptr_->at(*idx); + const auto & prev_ref_prev_front_point = prev_ref_points_ptr_->at(static_cast(*idx) - 1); + + ref_points.front().pose = prev_ref_prev_front_point.pose; + ref_points.front().fixed_kinematic_state = prev_ref_prev_front_point.optimized_kinematic_state; + ref_points.at(1).pose = prev_ref_front_point.pose; + ref_points.at(1).fixed_kinematic_state = prev_ref_front_point.optimized_kinematic_state; + } else { + // resample to make ref_points' interval constant. + // NOTE: Only pose, velocity and curvature will be interpolated. + ref_points = trajectory_utils::resampleReferencePoints(ref_points, mpt_param_.delta_arc_length); + + ref_points.front().pose = front_point.pose; + ref_points.front().curvature = front_point.curvature; + ref_points.front().fixed_kinematic_state = front_point.optimized_kinematic_state; } - const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const int nearest_prev_ref_idx = - static_cast(motion_utils::findFirstNearestIndexWithSoftConstraints( - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(prev_ref_points)), - current_ego_pose_, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold)); + time_keeper_ptr_->toc(__func__, " "); +} - // calculate begin_prev_ref_idx - const int begin_prev_ref_idx = [&]() { - const int backward_fixing_num = - traj_param_.backward_fixing_distance / mpt_param_.delta_arc_length_for_mpt_points; +void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points) const +{ + for (size_t i = 0; i < ref_points.size(); i++) { + ref_points.at(i).delta_arc_length = + (i == ref_points.size() - 1) + ? 0.0 + : tier4_autoware_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); + } +} - return std::max(0, nearest_prev_ref_idx - backward_fixing_num); - }(); +void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const +{ + // alpha + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto front_wheel_pos = + trajectory_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); - // calculate end_prev_ref_idx - const int end_prev_ref_idx = [&]() { - const double forward_fixed_length = std::max( - current_ego_vel_ * traj_param_.forward_fixing_min_time, - traj_param_.forward_fixing_min_distance); + const bool are_too_close_points = + tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; + const auto front_wheel_yaw = + are_too_close_points + ? ref_points.at(i).getYaw() + : tier4_autoware_utils::calcAzimuthAngle(ref_points.at(i).pose.position, front_wheel_pos); + ref_points.at(i).alpha = + tier4_autoware_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); + } - const int forward_fixing_num = - forward_fixed_length / mpt_param_.delta_arc_length_for_mpt_points; - return std::min( - static_cast(prev_ref_points.size()) - 1, nearest_prev_ref_idx + forward_fixing_num); - }(); + { // avoidance + // calculate one-step avoidance const + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto normalized_avoidance_cost = calcNormalizedAvoidanceCost(ref_points.at(i)); + if (normalized_avoidance_cost) { + const int max_length_idx = + std::floor(mpt_param_.avoidance_cost_band_length / mpt_param_.delta_arc_length); + for (int j = -max_length_idx; j <= max_length_idx; ++j) { + const int k = i + j; + if (0 <= k && k < static_cast(ref_points.size())) { + ref_points.at(k).normalized_avoidance_cost = *normalized_avoidance_cost; + } + } + } + } - bool points_reaching_prev_points_flag = false; - std::vector fixed_ref_points; - for (size_t i = begin_prev_ref_idx; i <= static_cast(end_prev_ref_idx); ++i) { - const auto & prev_ref_point = prev_ref_points.at(i); + /* + // update avoidance cost between longitudinally close obstacles + constexpr double max_longitudinal_length_to_fill_drivable_area = 50; + const int edge_fill_index = std::ceil(max_longitudinal_length_to_fill_drivable_area / + mpt_param_.delta_arc_length / 2); const auto copied_ref_points = ref_points; for (size_t i = 0; + i < ref_points.size(); ++i) { const double base_normalized_avoidance_cost = + ref_points.at(i).normalized_avoidance_cost; for (int j = -edge_fill_index; j <= edge_fill_index; + ++j) { const int k = i + j; if (k < 0 || ref_points.size() - 1 <= k) { continue; + } + ref_points.at(i).normalized_avoidance_cost = + std::max(ref_points.at(i).normalized_avoidance_cost, + copied_ref_points.at(k).normalized_avoidance_cost); + } + } + */ - if (!points_reaching_prev_points_flag) { - if (motion_utils::calcSignedArcLength(points, 0, prev_ref_point.p) < 0) { - continue; + // update spread avoidance cost + for (int i = 0; i < static_cast(ref_points.size()); ++i) { + const double base_normalized_avoidance_cost = ref_points.at(i).normalized_avoidance_cost; + if (0 < base_normalized_avoidance_cost) { + const int edge_decrease_idx = std::floor( + ref_points.at(i).normalized_avoidance_cost / mpt_param_.avoidance_cost_decrease_rate); + for (int j = -edge_decrease_idx; j <= edge_decrease_idx; ++j) { + const int k = i + j; + if (0 <= k && k < static_cast(ref_points.size())) { + const double normalized_avoidance_cost = std::max( + base_normalized_avoidance_cost - + std::abs(j) * mpt_param_.avoidance_cost_decrease_rate, + ref_points.at(k).normalized_avoidance_cost); + ref_points.at(k).normalized_avoidance_cost = + std::clamp(normalized_avoidance_cost, 0.0, 1.0); + } + } } - points_reaching_prev_points_flag = true; } - ReferencePoint fixed_ref_point; - fixed_ref_point = prev_ref_point; - fixed_ref_point.fix_kinematic_state = prev_ref_point.optimized_kinematic_state; + // take over previous avoidance cost + const double max_dist_threshold = mpt_param_.delta_arc_length / 2.0; + if (prev_ref_points_ptr_ && !prev_ref_points_ptr_->empty()) { + for (int i = 0; i < static_cast(ref_points.size()); ++i) { + const size_t prev_idx = trajectory_utils::findEgoIndex( + *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.at(i)), + ego_nearest_param_); + + const double dist_to_prev = tier4_autoware_utils::calcDistance2d( + ref_points.at(i), prev_ref_points_ptr_->at(prev_idx)); + if (max_dist_threshold < dist_to_prev) { + continue; + } - fixed_ref_points.push_back(fixed_ref_point); - if (mpt_param_.is_fixed_point_single) { - break; + ref_points.at(i).normalized_avoidance_cost = std::max( + prev_ref_points_ptr_->at(prev_idx).normalized_avoidance_cost, + ref_points.at(i).normalized_avoidance_cost); + } } } - - return fixed_ref_points; } -std::vector MPTOptimizer::getMPTFixedPoints( - const std::vector & ref_points) const +void MPTOptimizer::updateBounds( + std::vector & ref_points, + const std::vector & left_bound, + const std::vector & right_bound) const { - std::vector mpt_fixed_traj; - for (size_t i = 0; i < ref_points.size(); ++i) { - const auto & ref_point = ref_points.at(i); + time_keeper_ptr_->tic(__func__); + + const double soft_road_clearance = + mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; + + // calculate distance to left/right bound on each reference point + for (auto & ref_point : ref_points) { + const double dist_to_left_bound = + calcLateralDistToBounds(ref_point.pose, left_bound, soft_road_clearance, true); + const double dist_to_right_bound = + calcLateralDistToBounds(ref_point.pose, right_bound, soft_road_clearance, false); + ref_point.bounds = Bounds{dist_to_right_bound, dist_to_left_bound}; + } - if (ref_point.fix_kinematic_state) { - const double lat_error = ref_point.fix_kinematic_state.get()(0); - const double yaw_error = ref_point.fix_kinematic_state.get()(1); + /* + // TODO(murooka) deal with filling data between obstacles + // fill between obstacles + constexpr double max_longitudinal_length_to_fill_drivable_area = 20; + const int edge_fill_index = std::ceil(max_longitudinal_length_to_fill_drivable_area / + mpt_param_.delta_arc_length / 2); for (int i = 0; i < ref_points.size(); ++i) { for (int j = + -edge_fill_index; j <= edge_fill_index; ++j) { const int k = i + j; if (k < 0 || ref_points.size() + - 1 <= k) { continue; + } - autoware_auto_planning_msgs::msg::TrajectoryPoint fixed_traj_point; - fixed_traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0); - mpt_fixed_traj.push_back(fixed_traj_point); + const auto normalized_avoidance_cost = calcNormalizedAvoidanceCost(ref_points.at(k)); + if (normalized_avoidance_cost) { + } } } + */ - return mpt_fixed_traj; + time_keeper_ptr_->toc(__func__, " "); + return; } -// predict equation: x = Bex u + Wex (u includes x_0) -// cost function: J = x' Qex x + u' Rex u -MPTOptimizer::MPTMatrix MPTOptimizer::generateMPTMatrix( - const std::vector & ref_points, DebugData & debug_data) const +void MPTOptimizer::updateVehicleBounds( + std::vector & ref_points, + const SplineInterpolationPoints2d & ref_points_spline) const { - if (ref_points.empty()) { - return MPTMatrix{}; - } + time_keeper_ptr_->tic(__func__); - stop_watch_.tic(__func__); + for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { + const auto & ref_point = ref_points.at(p_idx); + // NOTE: This clear is required. + // It seems they sometimes already have previous values. + ref_points.at(p_idx).bounds_on_constraints.clear(); + ref_points.at(p_idx).beta.clear(); - // NOTE: center offset of vehicle is always 0 in this algorithm. - // vehicle_model_ptr_->updateCenterOffset(0.0); + for (const double lon_offset : vehicle_circle_longitudinal_offsets_) { + const auto collision_check_pose = + ref_points_spline.getSplineInterpolatedPose(p_idx, lon_offset); + const double collision_check_yaw = tf2::getYaw(collision_check_pose.orientation); - const size_t N_ref = ref_points.size(); - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t D_v = D_x + D_u * (N_ref - 1); + // calculate beta + const double beta = ref_point.getYaw() - collision_check_yaw; + ref_points.at(p_idx).beta.push_back(beta); - Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); - Eigen::VectorXd Wex = Eigen::VectorXd::Zero(D_x * N_ref); + // calculate vehicle_bounds_pose + const double tmp_yaw = std::atan2( + collision_check_pose.position.y - ref_point.pose.position.y, + collision_check_pose.position.x - ref_point.pose.position.x); + const double offset_y = + -tier4_autoware_utils::calcDistance2d(ref_point, collision_check_pose) * + std::sin(tmp_yaw - collision_check_yaw); - Eigen::MatrixXd Ad(D_x, D_x); - Eigen::MatrixXd Bd(D_x, D_u); - Eigen::MatrixXd Wd(D_x, 1); + const auto vehicle_bounds_pose = + tier4_autoware_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); - // predict kinematics for N_ref times - for (size_t i = 0; i < N_ref; ++i) { - if (i == 0) { - Bex.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); - continue; - } + // interpolate bounds + const auto bounds = [&]() { + const double collision_check_s = ref_points_spline.getAccumulatedLength(p_idx) + lon_offset; + const size_t collision_check_idx = ref_points_spline.getOffsetIndex(p_idx, lon_offset); - const int idx_x_i = i * D_x; - const int idx_x_i_prev = (i - 1) * D_x; - const int idx_u_i_prev = (i - 1) * D_u; + const size_t prev_idx = std::clamp( + collision_check_idx - 1, static_cast(0), + static_cast(ref_points_spline.getSize() - 2)); + const size_t next_idx = prev_idx + 1; - const double ds = [&]() { - if (N_ref == 1) { - return 0.0; - } - const size_t prev_idx = (i < N_ref - 1) ? i + 1 : i; - return ref_points.at(prev_idx).s - ref_points.at(prev_idx - 1).s; - }(); + const auto & prev_bounds = ref_points.at(prev_idx).bounds; + const auto & next_bounds = ref_points.at(next_idx).bounds; - // get discrete kinematics matrix A, B, W - const double ref_k = ref_points.at(std::max(0, static_cast(i) - 1)).k; - vehicle_model_ptr_->setCurvature(ref_k); - vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, ds); + const double prev_s = ref_points_spline.getAccumulatedLength(prev_idx); + const double next_s = ref_points_spline.getAccumulatedLength(next_idx); - Bex.block(idx_x_i, 0, D_x, D_x) = Ad * Bex.block(idx_x_i_prev, 0, D_x, D_x); - Bex.block(idx_x_i, D_x + idx_u_i_prev, D_x, D_u) = Bd; + const double ratio = std::clamp((collision_check_s - prev_s) / (next_s - prev_s), 0.0, 1.0); - for (size_t j = 0; j < i - 1; ++j) { - size_t idx_u_j = j * D_u; - Bex.block(idx_x_i, D_x + idx_u_j, D_x, D_u) = - Ad * Bex.block(idx_x_i_prev, D_x + idx_u_j, D_x, D_u); - } + auto bounds = Bounds::lerp(prev_bounds, next_bounds, ratio); + bounds.translate(offset_y); + return bounds; + }(); - Wex.segment(idx_x_i, D_x) = Ad * Wex.block(idx_x_i_prev, 0, D_x, 1) + Wd; + ref_points.at(p_idx).bounds_on_constraints.push_back(bounds); + ref_points.at(p_idx).pose_on_constraints.push_back(vehicle_bounds_pose); + } } - MPTMatrix m; - m.Bex = Bex; - m.Wex = Wex; - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return m; + time_keeper_ptr_->toc(__func__, " "); } -MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( +// cost function: J = x' Q x + u' R u +MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, - const std::vector & path_points, - DebugData & debug_data) const + const std::vector & traj_points) const { - if (ref_points.empty()) { - return ValueMatrix{}; - } - - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; - const bool is_containing_path_terminal_point = points_utils::isNearLastPathPoint( - ref_points.back(), path_points, traj_param_.max_dist_for_extending_end_point, - traj_param_.delta_yaw_threshold_for_closest_point); + const bool is_goal_contained = geometry_utils::isSamePoint(ref_points.back(), traj_points.back()); // update Q - Eigen::SparseMatrix Qex_sparse_mat(D_x * N_ref, D_x * N_ref); - std::vector> Qex_triplet_vec; + Eigen::SparseMatrix Q_sparse_mat(D_x * N_ref, D_x * N_ref); + std::vector> Q_triplet_vec; for (size_t i = 0; i < N_ref; ++i) { - // this is for plan_from_ego - // const bool near_kinematic_state_while_planning_from_ego = [&]() { - // const size_t min_idx = static_cast(std::max(0, static_cast(i) - 20)); - // const size_t max_idx = std::min(ref_points.size() - 1, i + 20); - // for (size_t j = min_idx; j <= max_idx; ++j) { - // if (ref_points.at(j).plan_from_ego && ref_points.at(j).fix_kinematic_state) { - // return true; - // } - // } - // return false; - // }(); - const auto adaptive_error_weight = [&]() -> std::array { - if (ref_points.at(i).near_objects) { - return { - mpt_param_.obstacle_avoid_lat_error_weight, mpt_param_.obstacle_avoid_yaw_error_weight}; - } else if (i == N_ref - 1 && is_containing_path_terminal_point) { - return { - mpt_param_.terminal_path_lat_error_weight, mpt_param_.terminal_path_yaw_error_weight}; - } else if (i == N_ref - 1) { + // for terminal point + if (i == N_ref - 1) { + if (is_goal_contained) { + return {mpt_param_.goal_lat_error_weight, mpt_param_.goal_yaw_error_weight}; + } return {mpt_param_.terminal_lat_error_weight, mpt_param_.terminal_yaw_error_weight}; } - // NOTE: may be better to add decreasing weights in a narrow and sharp curve - // else if (std::abs(ref_points[i].k) > 0.3) { - // return {0.0, 0.0}; - // } - + // for avoidance + if (0 < ref_points.at(i).normalized_avoidance_cost) { + const double lat_error_weight = interpolation::lerp( + mpt_param_.lat_error_weight, mpt_param_.avoidance_lat_error_weight, + ref_points.at(i).normalized_avoidance_cost); + const double yaw_error_weight = interpolation::lerp( + mpt_param_.yaw_error_weight, mpt_param_.avoidance_yaw_error_weight, + ref_points.at(i).normalized_avoidance_cost); + return {lat_error_weight, yaw_error_weight}; + } + // normal case return {mpt_param_.lat_error_weight, mpt_param_.yaw_error_weight}; }(); const double adaptive_lat_error_weight = adaptive_error_weight.at(0); const double adaptive_yaw_error_weight = adaptive_error_weight.at(1); - Qex_triplet_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, adaptive_lat_error_weight)); - Qex_triplet_vec.push_back( + Q_triplet_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, adaptive_lat_error_weight)); + Q_triplet_vec.push_back( Eigen::Triplet(i * D_x + 1, i * D_x + 1, adaptive_yaw_error_weight)); } - Qex_sparse_mat.setFromTriplets(Qex_triplet_vec.begin(), Qex_triplet_vec.end()); + Q_sparse_mat.setFromTriplets(Q_triplet_vec.begin(), Q_triplet_vec.end()); // update R - Eigen::SparseMatrix Rex_sparse_mat(D_v, D_v); - std::vector> Rex_triplet_vec; + Eigen::SparseMatrix R_sparse_mat(D_v, D_v); + std::vector> R_triplet_vec; for (size_t i = 0; i < N_ref - 1; ++i) { - const double adaptive_steer_weight = ref_points.at(i).near_objects - ? mpt_param_.obstacle_avoid_steer_input_weight - : mpt_param_.steer_input_weight; - Rex_triplet_vec.push_back( + const double adaptive_steer_weight = interpolation::lerp( + mpt_param_.steer_input_weight, mpt_param_.avoidance_steer_input_weight, + ref_points.at(i).normalized_avoidance_cost); + R_triplet_vec.push_back( Eigen::Triplet(D_x + D_u * i, D_x + D_u * i, adaptive_steer_weight)); } - addSteerWeightR(Rex_triplet_vec, ref_points); + addSteerWeightR(R_triplet_vec, ref_points); - Rex_sparse_mat.setFromTriplets(Rex_triplet_vec.begin(), Rex_triplet_vec.end()); + R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); ValueMatrix m; - m.Qex = Qex_sparse_mat; - m.Rex = Rex_sparse_mat; + m.Q = Q_sparse_mat; + m.R = R_sparse_mat; - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); return m; } -boost::optional MPTOptimizer::executeOptimization( - const std::unique_ptr & prev_trajs, const bool enable_avoidance, - const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, - const std::vector & ref_points, DebugData & debug_data) -{ - if (ref_points.empty()) { - return Eigen::VectorXd{}; - } - - stop_watch_.tic(__func__); - - const size_t N_ref = ref_points.size(); - - // get matrix - ObjectiveMatrix obj_m = getObjectiveMatrix(mpt_mat, val_mat, ref_points, debug_data); - ConstraintMatrix const_m = getConstraintMatrix(enable_avoidance, mpt_mat, ref_points, debug_data); - - // manual warm start - Eigen::VectorXd u0 = Eigen::VectorXd::Zero(obj_m.gradient.size()); - - if (mpt_param_.enable_manual_warm_start) { - const size_t D_x = vehicle_model_ptr_->getDimX(); - - if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) { - geometry_msgs::msg::Pose ref_front_point; - ref_front_point.position = ref_points.front().p; - ref_front_point.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_points.front().yaw); - - const size_t front_idx = findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(prev_trajs->mpt_ref_points), ref_front_point, - traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold); - - // set initial state - u0(0) = prev_trajs->mpt_ref_points.at(front_idx).optimized_kinematic_state(0); - u0(1) = prev_trajs->mpt_ref_points.at(front_idx).optimized_kinematic_state(1); - - // set steer angle - for (size_t i = 0; i + 1 < N_ref; ++i) { - const size_t prev_target_idx = - std::min(front_idx + i, prev_trajs->mpt_ref_points.size() - 1); - u0(D_x + i) = prev_trajs->mpt_ref_points.at(prev_target_idx).optimized_input; - } - } - } - - const Eigen::MatrixXd & H = obj_m.hessian; - const Eigen::MatrixXd & A = const_m.linear; - std::vector f; - std::vector upper_bound; - std::vector lower_bound; - - if (mpt_param_.enable_manual_warm_start) { - f = eigenVectorToStdVector(obj_m.gradient + H * u0); - Eigen::VectorXd A_times_u0 = A * u0; - upper_bound = eigenVectorToStdVector(const_m.upper_bound - A_times_u0); - lower_bound = eigenVectorToStdVector(const_m.lower_bound - A_times_u0); - } else { - f = eigenVectorToStdVector(obj_m.gradient); - upper_bound = eigenVectorToStdVector(const_m.upper_bound); - lower_bound = eigenVectorToStdVector(const_m.lower_bound); - } - - // initialize or update solver with warm start - stop_watch_.tic("initOsqp"); - autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); - autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); - if (mpt_param_.enable_warm_start && prev_mat_n == H.rows() && prev_mat_m == A.rows()) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "warm start"); - - osqp_solver_ptr_->updateCscP(P_csc); - osqp_solver_ptr_->updateQ(f); - osqp_solver_ptr_->updateCscA(A_csc); - osqp_solver_ptr_->updateL(lower_bound); - osqp_solver_ptr_->updateU(upper_bound); - } else { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "no warm start"); - - osqp_solver_ptr_ = std::make_unique( - // obj_m.hessian, const_m.linear, obj_m.gradient, const_m.lower_bound, const_m.upper_bound, - P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); - } - prev_mat_n = H.rows(); - prev_mat_m = A.rows(); - - debug_data.msg_stream << " " - << "initOsqp" - << ":= " << stop_watch_.toc("initOsqp") << " [ms]\n"; - - // solve - stop_watch_.tic("solveOsqp"); - const auto result = osqp_solver_ptr_->optimize(); - debug_data.msg_stream << " " - << "solveOsqp" - << ":= " << stop_watch_.toc("solveOsqp") << " [ms]\n"; - - // check solution status - const int solution_status = std::get<3>(result); - if (solution_status != 1) { - utils::logOSQPSolutionStatus(solution_status, "MPT: "); - return boost::none; - } - - // print iteration - const int iteration_status = std::get<4>(result); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "iteration: %d", iteration_status); - - // get result - std::vector result_vec = std::get<0>(result); - - const size_t DIM_U = vehicle_model_ptr_->getDimU(); - const size_t DIM_X = vehicle_model_ptr_->getDimX(); - const Eigen::VectorXd optimized_control_variables = - Eigen::Map(&result_vec[0], DIM_X + (N_ref - 1) * DIM_U); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - - const Eigen::VectorXd optimized_control_variables_with_offset = - mpt_param_.enable_manual_warm_start - ? optimized_control_variables + u0.segment(0, DIM_X + (N_ref - 1) * DIM_U) - : optimized_control_variables; - - return optimized_control_variables_with_offset; -} - -MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( - const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, - [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const +MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( + const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, + const std::vector & ref_points) const { - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); const size_t N_ref = ref_points.size(); - + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); const size_t D_xn = D_x * N_ref; const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_slack = getNumberOfSlackVariables(); // generate T matrix and vector to shift optimization center - // define Z as time-series vector of shifted deviation error - // Z = sparse_T_mat * (Bex * U + Wex) + T_vec + // NOTE: Z is defined as time-series vector of shifted deviation + // error where Z = sparse_T_mat * (B * U + W) + T_vec Eigen::SparseMatrix sparse_T_mat(D_xn, D_xn); Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(D_xn); std::vector> triplet_T_vec; @@ -907,212 +973,158 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( } sparse_T_mat.setFromTriplets(triplet_T_vec.begin(), triplet_T_vec.end()); - const Eigen::MatrixXd B = sparse_T_mat * mpt_mat.Bex; - const Eigen::MatrixXd QB = val_mat.Qex * B; - const Eigen::MatrixXd R = val_mat.Rex; + const Eigen::MatrixXd B = sparse_T_mat * mpt_mat.B; + const Eigen::MatrixXd QB = val_mat.Q * B; + const Eigen::MatrixXd R = val_mat.R; - // min J(v) = min (v'Hv + v'f) + // calculate H, and extend it for slack variables + // NOTE: min J(v) = min (v'Hv + v'g) Eigen::MatrixXd H = Eigen::MatrixXd::Zero(D_v, D_v); H.triangularView() = B.transpose() * QB + R; H.triangularView() = H.transpose(); - // Eigen::VectorXd f = ((sparse_T_mat * mpt_mat.Wex + T_vec).transpose() * QB).transpose(); - Eigen::VectorXd f = (sparse_T_mat * mpt_mat.Wex + T_vec).transpose() * QB; - - const size_t N_avoid = mpt_param_.vehicle_circle_longitudinal_offsets.size(); - const size_t N_first_slack = [&]() -> size_t { - if (mpt_param_.soft_constraint) { - if (mpt_param_.l_inf_norm) { - return 1; - } - return N_avoid; - } - return 0; - }(); - const size_t N_second_slack = [&]() -> size_t { - if (mpt_param_.two_step_soft_constraint) { - return N_first_slack; - } - return 0; - }(); - - // number of slack variables for one step - const size_t N_slack = N_first_slack + N_second_slack; + Eigen::MatrixXd extended_H = Eigen::MatrixXd::Zero(D_v + N_ref * N_slack, D_v + N_ref * N_slack); + extended_H.block(0, 0, D_v, D_v) = H; - // extend H for slack variables - Eigen::MatrixXd full_H = Eigen::MatrixXd::Zero(D_v + N_ref * N_slack, D_v + N_ref * N_slack); - full_H.block(0, 0, D_v, D_v) = H; - - // extend f for slack variables - Eigen::VectorXd full_f(D_v + N_ref * N_slack); + // calculate g, and extend it for slack variables + Eigen::VectorXd g = (sparse_T_mat * mpt_mat.W + T_vec).transpose() * QB; + /* + Eigen::VectorXd extended_g(D_v + N_ref * N_slack); - full_f.segment(0, D_v) = f; - if (N_first_slack > 0) { - full_f.segment(D_v, N_ref * N_first_slack) = - mpt_param_.soft_avoidance_weight * Eigen::VectorXd::Ones(N_ref * N_first_slack); - } - if (N_second_slack > 0) { - full_f.segment(D_v + N_ref * N_first_slack, N_ref * N_second_slack) = - mpt_param_.soft_second_avoidance_weight * Eigen::VectorXd::Ones(N_ref * N_second_slack); + extended_g.segment(0, D_v) = g; + if (N_slack > 0) { + extended_g.segment(D_v, N_ref * N_slack) = + mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); } + */ + Eigen::VectorXd extended_g(D_v + N_ref * N_slack); + extended_g << g, mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); ObjectiveMatrix obj_matrix; - obj_matrix.hessian = full_H; - obj_matrix.gradient = full_f; - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + obj_matrix.hessian = extended_H; + obj_matrix.gradient = extended_g; + time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } -// Set constraint: lb <= Ax <= ub +// Constraint: lb <= A u <= ub // decision variable -// x := [u0, ..., uN-1 | z00, ..., z0N-1 | z10, ..., z1N-1 | z20, ..., z2N-1] -// \in \mathbb{R}^{N * (N_vehicle_circle + 1)} -MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( - [[maybe_unused]] const bool enable_avoidance, const MPTMatrix & mpt_mat, - const std::vector & ref_points, [[maybe_unused]] DebugData & debug_data) const +// u := [initial state, steer angles, soft variables] +MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( + const StateEquationGenerator::Matrix & mpt_mat, + const std::vector & ref_points) const { - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - // NOTE: currently, add additional length to soft bounds approximately - // for soft second and hard bounds - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); const size_t N_ref = ref_points.size(); - const size_t N_u = (N_ref - 1) * D_u; const size_t D_v = D_x + N_u; + const size_t N_collision_check = vehicle_circle_longitudinal_offsets_.size(); - const size_t N_avoid = mpt_param_.vehicle_circle_longitudinal_offsets.size(); - - // number of slack variables for one step - const size_t N_first_slack = [&]() -> size_t { - if (mpt_param_.soft_constraint) { - if (mpt_param_.l_inf_norm) { - return 1; - } - return N_avoid; - } - return 0; - }(); - const size_t N_second_slack = [&]() -> size_t { - if (mpt_param_.soft_constraint && mpt_param_.two_step_soft_constraint) { - return N_first_slack; - } - return 0; - }(); - - // number of all slack variables is N_ref * N_slack - const size_t N_slack = N_first_slack + N_second_slack; - const size_t N_soft = mpt_param_.two_step_soft_constraint ? 2 : 1; - - const size_t A_cols = [&] { - if (mpt_param_.soft_constraint) { - return D_v + N_ref * N_slack; // initial_state + steer + soft - } - return D_v; // initial state + steer - }(); + // NOTE: The number of one-step slack variables. + // The number of all slack variables will be N_ref * N_slack. + const size_t N_slack = getNumberOfSlackVariables(); // calculate indices of fixed points std::vector fixed_points_indices; for (size_t i = 0; i < N_ref; ++i) { - if (ref_points.at(i).fix_kinematic_state) { + if (ref_points.at(i).fixed_kinematic_state) { fixed_points_indices.push_back(i); } } - // calculate rows of A + // calculate rows and cols of A size_t A_rows = 0; if (mpt_param_.soft_constraint) { - // 3 means slack variable constraints to be between lower and upper bounds, and positive. - A_rows += 3 * N_ref * N_avoid * N_soft; + // NOTE: 3 means expecting slack variable constraints to be larger than lower bound, + // smaller than upper bound, and positive. + A_rows += 3 * N_ref * N_collision_check; } if (mpt_param_.hard_constraint) { - A_rows += N_ref * N_avoid; + A_rows += N_ref * N_collision_check; } A_rows += fixed_points_indices.size() * D_x; if (mpt_param_.steer_limit_constraint) { A_rows += N_u; } + const size_t A_cols = [&] { + if (mpt_param_.soft_constraint) { + return D_v + N_ref * N_slack; // initial state + steer angles + soft variables + } + return D_v; // initial state + steer angles + }(); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, A_cols); Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); size_t A_rows_end = 0; // CX = C(Bv + w) + C \in R^{N_ref, N_ref * D_x} - for (size_t l_idx = 0; l_idx < N_avoid; ++l_idx) { - // create C := [1 | l | O] + for (size_t l_idx = 0; l_idx < N_collision_check; ++l_idx) { + // create C := [cos(beta) | l cos(beta)] Eigen::SparseMatrix C_sparse_mat(N_ref, N_ref * D_x); std::vector> C_triplet_vec; Eigen::VectorXd C_vec = Eigen::VectorXd::Zero(N_ref); // calculate C mat and vec for (size_t i = 0; i < N_ref; ++i) { - const double beta = ref_points.at(i).beta.at(l_idx).get(); - const double avoid_offset = mpt_param_.vehicle_circle_longitudinal_offsets.at(l_idx); + const double beta = *ref_points.at(i).beta.at(l_idx); + const double lon_offset = vehicle_circle_longitudinal_offsets_.at(l_idx); C_triplet_vec.push_back(Eigen::Triplet(i, i * D_x, 1.0 * std::cos(beta))); - C_triplet_vec.push_back( - Eigen::Triplet(i, i * D_x + 1, avoid_offset * std::cos(beta))); - C_vec(i) = avoid_offset * std::sin(beta); + C_triplet_vec.push_back(Eigen::Triplet(i, i * D_x + 1, lon_offset * std::cos(beta))); + C_vec(i) = lon_offset * std::sin(beta); } C_sparse_mat.setFromTriplets(C_triplet_vec.begin(), C_triplet_vec.end()); // calculate CB, and CW - const Eigen::MatrixXd CB = C_sparse_mat * mpt_mat.Bex; - const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.Wex + C_vec; + const Eigen::MatrixXd CB = C_sparse_mat * mpt_mat.B; + const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.W + C_vec; // calculate bounds - const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx, vehicle_param_); + const double bounds_offset = + vehicle_info_.vehicle_width_m / 2.0 - vehicle_circle_radiuses_.at(l_idx); + const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx, bounds_offset); // soft constraints if (mpt_param_.soft_constraint) { size_t A_offset_cols = D_v; - for (size_t s_idx = 0; s_idx < N_soft; ++s_idx) { - const size_t A_blk_rows = 3 * N_ref; - - // A := [C * Bex | O | ... | O | I | O | ... - // -C * Bex | O | ... | O | I | O | ... - // O | O | ... | O | I | O | ... ] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); - A_blk.block(0, 0, N_ref, D_v) = CB; - A_blk.block(N_ref, 0, N_ref, D_v) = -CB; - - size_t local_A_offset_cols = A_offset_cols; - if (!mpt_param_.l_inf_norm) { - local_A_offset_cols += N_ref * l_idx; - } - A_blk.block(0, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, local_A_offset_cols, N_ref, N_ref) = - Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, local_A_offset_cols, N_ref, N_ref) = - Eigen::MatrixXd::Identity(N_ref, N_ref); - - // lb := [lower_bound - CW - // CW - upper_bound - // O ] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(A_blk_rows); - lb_blk.segment(0, N_ref) = -CW + part_lb; - lb_blk.segment(N_ref, N_ref) = CW - part_ub; - - if (s_idx == 1) { - // add additional clearance - const double diff_clearance = - mpt_param_.soft_second_clearance_from_road - mpt_param_.soft_clearance_from_road; - lb_blk.segment(0, N_ref) -= Eigen::MatrixXd::Constant(N_ref, 1, diff_clearance); - lb_blk.segment(N_ref, N_ref) -= Eigen::MatrixXd::Constant(N_ref, 1, diff_clearance); - } - - A_offset_cols += N_ref * N_first_slack; + const size_t A_blk_rows = 3 * N_ref; - A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; - lb.segment(A_rows_end, A_blk_rows) = lb_blk; + // A := [C * B | O | ... | O | I | O | ... + // -C * B | O | ... | O | I | O | ... + // O | O | ... | O | I | O | ... ] + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); + A_blk.block(0, 0, N_ref, D_v) = CB; + A_blk.block(N_ref, 0, N_ref, D_v) = -CB; - A_rows_end += A_blk_rows; + size_t local_A_offset_cols = A_offset_cols; + if (!mpt_param_.l_inf_norm) { + local_A_offset_cols += N_ref * l_idx; } + A_blk.block(0, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); + A_blk.block(N_ref, local_A_offset_cols, N_ref, N_ref) = + Eigen::MatrixXd::Identity(N_ref, N_ref); + A_blk.block(2 * N_ref, local_A_offset_cols, N_ref, N_ref) = + Eigen::MatrixXd::Identity(N_ref, N_ref); + + // lb := [lower_bound - CW + // CW - upper_bound + // O ] + Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(A_blk_rows); + lb_blk.segment(0, N_ref) = -CW + part_lb; + lb_blk.segment(N_ref, N_ref) = CW - part_ub; + + A_offset_cols += N_ref * N_slack; + + A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; + lb.segment(A_rows_end, A_blk_rows) = lb_blk; + + A_rows_end += A_blk_rows; } // hard constraints @@ -1131,25 +1143,33 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( } // fixed points constraint - // CX = C(B v + w) where C extracts fixed points - if (fixed_points_indices.size() > 0) { - for (const size_t i : fixed_points_indices) { - A.block(A_rows_end, 0, D_x, D_v) = mpt_mat.Bex.block(i * D_x, 0, D_x, D_v); + // X = B v + w where point is fixed + for (const size_t i : fixed_points_indices) { + A.block(A_rows_end, 0, D_x, D_v) = mpt_mat.B.block(i * D_x, 0, D_x, D_v); - lb.segment(A_rows_end, D_x) = - ref_points[i].fix_kinematic_state.get() - mpt_mat.Wex.segment(i * D_x, D_x); - ub.segment(A_rows_end, D_x) = - ref_points[i].fix_kinematic_state.get() - mpt_mat.Wex.segment(i * D_x, D_x); + lb.segment(A_rows_end, D_x) = + ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); + ub.segment(A_rows_end, D_x) = + ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); - A_rows_end += D_x; - } + A_rows_end += D_x; } - // steer max limit + // steer limit if (mpt_param_.steer_limit_constraint) { A.block(A_rows_end, D_x, N_u, N_u) = Eigen::MatrixXd::Identity(N_u, N_u); - lb.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, -mpt_param_.max_steer_rad); - ub.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, mpt_param_.max_steer_rad); + + // TODO(murooka) use curvature by stabling optimization + // Currently, when using curvature, the optimization result is weird with sample_map. + // lb.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, -mpt_param_.max_steer_rad); + // ub.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, mpt_param_.max_steer_rad); + + for (size_t i = 0; i < N_u; ++i) { + const double ref_steer_angle = + std::atan2(vehicle_info_.wheel_base_m * ref_points.at(i).curvature, 1.0); + lb(A_rows_end + i) = ref_steer_angle - mpt_param_.max_steer_rad; + ub(A_rows_end + i) = ref_steer_angle + mpt_param_.max_steer_rad; + } A_rows_end += N_u; } @@ -1159,384 +1179,312 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( constraint_matrix.lower_bound = lb; constraint_matrix.upper_bound = ub; - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); return constraint_matrix; } -std::vector MPTOptimizer::getMPTPoints( - std::vector & fixed_ref_points, - std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_mat, DebugData & debug_data) +void MPTOptimizer::addSteerWeightR( + std::vector> & R_triplet_vec, + const std::vector & ref_points) const { - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t N_ref = static_cast(Uex.rows() - D_x) + 1; - - stop_watch_.tic(__func__); - - std::vector lat_error_vec; - std::vector yaw_error_vec; - for (size_t i = 0; i < fixed_ref_points.size(); ++i) { - const auto & ref_point = fixed_ref_points.at(i); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t D_v = D_x + (N_ref - 1) * D_u; - lat_error_vec.push_back(ref_point.fix_kinematic_state.get()(0)); - yaw_error_vec.push_back(ref_point.fix_kinematic_state.get()(1)); + // add steering rate : weight for (u(i) - u(i-1))^2 + for (size_t i = D_x; i < D_v - 1; ++i) { + R_triplet_vec.push_back(Eigen::Triplet(i, i, mpt_param_.steer_rate_weight)); + R_triplet_vec.push_back(Eigen::Triplet(i + 1, i, -mpt_param_.steer_rate_weight)); + R_triplet_vec.push_back(Eigen::Triplet(i, i + 1, -mpt_param_.steer_rate_weight)); + R_triplet_vec.push_back(Eigen::Triplet(i + 1, i + 1, mpt_param_.steer_rate_weight)); } +} - const size_t N_kinematic_state = vehicle_model_ptr_->getDimX(); - const Eigen::VectorXd Xex = mpt_mat.Bex * Uex + mpt_mat.Wex; - - for (size_t i = 0; i < non_fixed_ref_points.size(); ++i) { - lat_error_vec.push_back(Xex(i * N_kinematic_state)); - yaw_error_vec.push_back(Xex(i * N_kinematic_state + 1)); - } +std::optional MPTOptimizer::calcOptimizedSteerAngles( + const std::vector & ref_points, const ObjectiveMatrix & obj_mat, + const ConstraintMatrix & const_mat) +{ + time_keeper_ptr_->tic(__func__); - // calculate trajectory from optimization result - std::vector traj_points; - debug_data.vehicle_circles_pose.resize(lat_error_vec.size()); - for (size_t i = 0; i < lat_error_vec.size(); ++i) { - auto & ref_point = (i < fixed_ref_points.size()) - ? fixed_ref_points.at(i) - : non_fixed_ref_points.at(i - fixed_ref_points.size()); - const double lat_error = lat_error_vec.at(i); - const double yaw_error = yaw_error_vec.at(i); - - geometry_msgs::msg::Pose ref_pose; - ref_pose.position = ref_point.p; - ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); - debug_data.mpt_ref_poses.push_back(ref_pose); - debug_data.lateral_errors.push_back(lat_error); - debug_data.yaw_errors.push_back(yaw_error); - - ref_point.optimized_kinematic_state << lat_error_vec.at(i), yaw_error_vec.at(i); - if (i >= fixed_ref_points.size()) { - const size_t j = i - fixed_ref_points.size(); - if (j == N_ref - 1) { - ref_point.optimized_input = 0.0; - } else { - ref_point.optimized_input = Uex(D_x + j * D_u); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_slack = getNumberOfSlackVariables(); + const size_t D_un = D_v + N_ref * N_slack; + + // for manual warm start, calculate initial solution + const auto u0 = [&]() -> std::optional { + if (mpt_param_.enable_manual_warm_start) { + if (prev_ref_points_ptr_ && 1 < prev_ref_points_ptr_->size()) { + return calcInitialSolutionForManualWarmStart(ref_points, *prev_ref_points_ptr_); } } + return std::nullopt; + }(); - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0); - - traj_points.push_back(traj_point); - - { // for debug visualization - const double base_x = ref_point.p.x - std::sin(ref_point.yaw) * lat_error; - const double base_y = ref_point.p.y + std::cos(ref_point.yaw) * lat_error; - - // NOTE: coordinate of vehicle_circle_longitudinal_offsets is back wheel center - for (const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { - geometry_msgs::msg::Pose vehicle_circle_pose; + // for manual start, update objective and constraint matrix + const auto [updated_obj_mat, updated_const_mat] = + updateMatrixForManualWarmStart(obj_mat, const_mat, u0); + + // calculate matrices for qp + const Eigen::MatrixXd & H = updated_obj_mat.hessian; + const Eigen::MatrixXd & A = updated_const_mat.linear; + const auto f = toStdVector(updated_obj_mat.gradient); + const auto upper_bound = toStdVector(updated_const_mat.upper_bound); + const auto lower_bound = toStdVector(updated_const_mat.lower_bound); + + // initialize or update solver according to warm start + time_keeper_ptr_->tic("initOsqp"); + + const autoware::common::osqp::CSC_Matrix P_csc = + autoware::common::osqp::calCSCMatrixTrapezoidal(H); + const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); + if (mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) { + RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "warm start"); + osqp_solver_ptr_->updateCscP(P_csc); + osqp_solver_ptr_->updateQ(f); + osqp_solver_ptr_->updateCscA(A_csc); + osqp_solver_ptr_->updateL(lower_bound); + osqp_solver_ptr_->updateU(upper_bound); + } else { + RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "no warm start"); + osqp_solver_ptr_ = std::make_unique( + P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); + } + prev_mat_n_ = H.rows(); + prev_mat_m_ = A.rows(); - vehicle_circle_pose.position.x = base_x + d * std::cos(ref_point.yaw + yaw_error); - vehicle_circle_pose.position.y = base_y + d * std::sin(ref_point.yaw + yaw_error); + time_keeper_ptr_->toc("initOsqp", " "); - vehicle_circle_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + ref_point.alpha); + // solve qp + time_keeper_ptr_->tic("solveOsqp"); + const auto result = osqp_solver_ptr_->optimize(); + time_keeper_ptr_->toc("solveOsqp", " "); - debug_data.vehicle_circles_pose.at(i).push_back(vehicle_circle_pose); - } - } + // check solution status + const int solution_status = std::get<3>(result); + if (solution_status != 1) { + osqp_solver_ptr_->logUnsolvedStatus("[MPT]"); + return std::nullopt; } - // NOTE: If generated trajectory's orientation is not so smooth or kinematically infeasible, - // recalculate orientation here for (size_t i = 0; i < lat_error_vec.size(); ++i) { - // auto & ref_point = (i < fixed_ref_points.size()) - // ? fixed_ref_points.at(i) - // : non_fixed_ref_points.at(i - fixed_ref_points.size()); - // - // if (i > 0 && traj_points.size() > 1) { - // traj_points.at(i).pose.orientation = geometry_utils::getQuaternionFromPoints( - // traj_points.at(i).pose.position, traj_points.at(i - 1).pose.position); - // } else if (traj_points.size() > 1) { - // traj_points.at(i).pose.orientation = geometry_utils::getQuaternionFromPoints( - // traj_points.at(i + 1).pose.position, traj_points.at(i).pose.position); - // } else { - // traj_points.at(i).pose.orientation = - // tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); - // } - // } - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + // print iteration + const int iteration_status = std::get<4>(result); + RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "iteration: %d", iteration_status); - return traj_points; -} + // get optimization result + auto optimization_result = + std::get<0>(result); // NOTE: const cannot be added due to the next operation. + const Eigen::VectorXd optimized_steer_angles = + Eigen::Map(&optimization_result[0], D_un); -void MPTOptimizer::calcOrientation(std::vector & ref_points) const -{ - const auto yaw_angles = splineYawFromReferencePoints(ref_points); - for (size_t i = 0; i < ref_points.size(); ++i) { - if (ref_points.at(i).fix_kinematic_state) { - continue; - } + time_keeper_ptr_->toc(__func__, " "); - ref_points.at(i).yaw = yaw_angles.at(i); + if (u0) { // manual warm start + return static_cast(optimized_steer_angles + *u0); } + return optimized_steer_angles; } -void MPTOptimizer::calcCurvature(std::vector & ref_points) const +Eigen::VectorXd MPTOptimizer::calcInitialSolutionForManualWarmStart( + const std::vector & ref_points, + const std::vector & prev_ref_points) const { - const size_t num_points = static_cast(ref_points.size()); - - // calculate curvature by circle fitting from three points - size_t max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); - size_t L = - std::min(static_cast(mpt_param_.num_curvature_sampling_points), max_smoothing_num); - auto curvatures = points_utils::calcCurvature( - ref_points, static_cast(mpt_param_.num_curvature_sampling_points)); - for (size_t i = L; i < num_points - L; ++i) { - if (!ref_points.at(i).fix_kinematic_state) { - ref_points.at(i).k = curvatures.at(i); - } + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t N_u = (N_ref - 1) * D_u; + const size_t D_v = D_x + N_u; + const size_t N_slack = getNumberOfSlackVariables(); + const size_t D_un = D_v + N_ref * N_slack; + + Eigen::VectorXd u0 = Eigen::VectorXd::Zero(D_un); + + const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + prev_ref_points, ref_points.front().pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); + + // set previous lateral and yaw deviation + u0(0) = prev_ref_points.at(nearest_idx).optimized_kinematic_state.lat; + u0(1) = prev_ref_points.at(nearest_idx).optimized_kinematic_state.yaw; + + // set previous steer angles + for (size_t i = 0; i < N_u; ++i) { + const size_t prev_target_idx = std::min(nearest_idx + i, prev_ref_points.size() - 1); + u0(D_x + i) = prev_ref_points.at(prev_target_idx).optimized_input; } - // first and last curvature is copied from next value - for (size_t i = 0; i < std::min(L, num_points); ++i) { - if (!ref_points.at(i).fix_kinematic_state) { - ref_points.at(i).k = ref_points.at(std::min(L, num_points - 1)).k; - } - if (!ref_points.at(num_points - i - 1).fix_kinematic_state) { - ref_points.at(num_points - i - 1).k = - ref_points.at(std::max(static_cast(num_points) - static_cast(L) - 1, 0)).k; + + // set previous slack variables + for (size_t i = 0; i < N_ref; ++i) { + const auto & slack_variables = ref_points.at(i).slack_variables; + if (slack_variables) { + for (size_t j = 0; j < slack_variables->size(); ++j) { + u0(D_v + i * N_slack + j) = slack_variables->at(j); + } } } + + return u0; } -void MPTOptimizer::calcArcLength(std::vector & ref_points) const +std::pair +MPTOptimizer::updateMatrixForManualWarmStart( + const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat, + const std::optional & u0) const { - for (size_t i = 0; i < ref_points.size(); i++) { - if (i > 0) { - geometry_msgs::msg::Point a, b; - a = ref_points.at(i).p; - b = ref_points.at(i - 1).p; - ref_points.at(i).s = ref_points.at(i - 1).s + tier4_autoware_utils::calcDistance2d(a, b); - } + if (!u0) { + // not manual warm start + return {obj_mat, const_mat}; } -} -void MPTOptimizer::calcExtraPoints( - std::vector & ref_points, const std::unique_ptr & prev_trajs) const -{ - for (size_t i = 0; i < ref_points.size(); ++i) { - // alpha - const double front_wheel_s = - ref_points.at(i).s + - vehicle_param_.wheelbase; // TODO(murooka) use offset instead of wheelbase? - const int front_wheel_nearest_idx = points_utils::getNearestIdx(ref_points, front_wheel_s, i); - const auto front_wheel_pos = ref_points.at(front_wheel_nearest_idx).p; + const Eigen::MatrixXd & H = obj_mat.hessian; + const Eigen::MatrixXd & A = const_mat.linear; - const bool are_too_close_points = - tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).p) < 1e-03; - const auto front_wheel_yaw = are_too_close_points ? ref_points.at(i).yaw - : tier4_autoware_utils::calcAzimuthAngle( - ref_points.at(i).p, front_wheel_pos); - ref_points.at(i).alpha = - tier4_autoware_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).yaw); + auto updated_obj_mat = obj_mat; + auto updated_const_mat = const_mat; - // near objects - ref_points.at(i).near_objects = [&]() { - const int avoidance_check_steps = - mpt_param_.near_objects_length / mpt_param_.delta_arc_length_for_mpt_points; + Eigen::VectorXd & f = updated_obj_mat.gradient; + Eigen::VectorXd & ub = updated_const_mat.upper_bound; + Eigen::VectorXd & lb = updated_const_mat.lower_bound; - const int avoidance_check_begin_idx = - std::max(0, static_cast(i) - avoidance_check_steps); - const int avoidance_check_end_idx = - std::min(static_cast(ref_points.size()), static_cast(i) + avoidance_check_steps); + // update gradient + f += H * *u0; - for (int a_idx = avoidance_check_begin_idx; a_idx < avoidance_check_end_idx; ++a_idx) { - if (ref_points.at(a_idx).vehicle_bounds.at(0).hasCollisionWithObject()) { - return true; - } - } - return false; - }(); + // update upper_bound and lower_bound + const Eigen::VectorXd A_times_u0 = A * *u0; + ub -= A_times_u0; + lb -= A_times_u0; - // The point are considered to be near the object if nearest previous ref point is near the - // object. - if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { - const auto & prev_ref_points = prev_trajs->mpt_ref_points; - - const auto ref_points_with_yaw = - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); - const size_t prev_idx = findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(prev_ref_points), ref_points_with_yaw.at(i), - traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - const double dist_to_nearest_prev_ref = - tier4_autoware_utils::calcDistance2d(prev_ref_points.at(prev_idx), ref_points.at(i)); - if (dist_to_nearest_prev_ref < 1.0 && prev_ref_points.at(prev_idx).near_objects) { - ref_points.at(i).near_objects = true; - } - } - } + return {updated_obj_mat, updated_const_mat}; } -void MPTOptimizer::addSteerWeightR( - std::vector> & Rex_triplet_vec, - const std::vector & ref_points) const +std::optional> MPTOptimizer::calcMPTPoints( + std::vector & ref_points, const Eigen::VectorXd & U, + const StateEquationGenerator::Matrix & mpt_mat) const { - const size_t D_x = vehicle_model_ptr_->getDimX(); - const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t N_ref = ref_points.size(); - const size_t N_u = (N_ref - 1) * D_u; - const size_t D_v = D_x + N_u; - - // add steering rate : weight for (u(i) - u(i-1))^2 - for (size_t i = D_x; i < D_v - 1; ++i) { - Rex_triplet_vec.push_back(Eigen::Triplet(i, i, mpt_param_.steer_rate_weight)); - Rex_triplet_vec.push_back(Eigen::Triplet(i + 1, i, -mpt_param_.steer_rate_weight)); - Rex_triplet_vec.push_back(Eigen::Triplet(i, i + 1, -mpt_param_.steer_rate_weight)); - Rex_triplet_vec.push_back(Eigen::Triplet(i + 1, i + 1, mpt_param_.steer_rate_weight)); - } -} + time_keeper_ptr_->tic(__func__); -void MPTOptimizer::calcBounds( - std::vector & ref_points, - const std::vector & left_bound, - const std::vector & right_bound, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); + const size_t D_x = state_equation_generator_.getDimX(); + const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_slack = getNumberOfSlackVariables(); - if (left_bound.empty() || right_bound.empty()) { - std::cerr << "[ObstacleAvoidancePlanner]: Boundary is empty when calculating bounds" - << std::endl; - return; - } + const Eigen::VectorXd steer_angles = U.segment(0, D_v); + const Eigen::VectorXd slack_variables = U.segment(D_v, N_ref * N_slack); - /* - const double min_soft_road_clearance = vehicle_param_.width / 2.0 + - mpt_param_.soft_clearance_from_road + - mpt_param_.extra_desired_clearance_from_road; - */ - const double base_to_right = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.right_overhang; - const double base_to_left = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.left_overhang; - - // search bounds candidate for each ref points - debug_data.bounds_candidates.clear(); - for (size_t i = 0; i < ref_points.size() - 1; ++i) { - const auto curr_ref_position = convertRefPointsToPose(ref_points.at(i)).position; - const auto next_ref_position = convertRefPointsToPose(ref_points.at(i + 1)).position; - const Eigen::Vector2d current_ref_point = {curr_ref_position.x, curr_ref_position.y}; - const Eigen::Vector2d next_ref_point = {next_ref_position.x, next_ref_position.y}; - const Eigen::Vector2d current_to_next_vec = next_ref_point - current_ref_point; - const Eigen::Vector2d left_vertical_vec = {-current_to_next_vec(1), current_to_next_vec(0)}; - const Eigen::Vector2d right_vertical_vec = {current_to_next_vec(1), -current_to_next_vec(0)}; - - const Eigen::Vector2d left_point = current_ref_point + left_vertical_vec.normalized() * 5.0; - const Eigen::Vector2d right_point = current_ref_point + right_vertical_vec.normalized() * 5.0; - const double lat_dist_to_left_bound = std::min( - calcLateralDistToBound(current_ref_point, left_point, left_bound) - base_to_left, 5.0); - const double lat_dist_to_right_bound = std::max( - calcLateralDistToBound(current_ref_point, right_point, right_bound, true) + base_to_right, - -5.0); - - ref_points.at(i).bounds = Bounds{ - lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, - CollisionType::NO_COLLISION}; - debug_data.bounds_candidates.push_back(ref_points.at(i).bounds); - } + // predict time-series states from optimized control inputs + const Eigen::VectorXd X = state_equation_generator_.predict(mpt_mat, steer_angles); - // Terminal Boundary - const double lat_dist_to_left_bound = - -motion_utils::calcLateralOffset( - left_bound, convertRefPointsToPose(ref_points.back()).position) - - base_to_left; - const double lat_dist_to_right_bound = - -motion_utils::calcLateralOffset( - right_bound, convertRefPointsToPose(ref_points.back()).position) + - base_to_right; - ref_points.back().bounds = Bounds{ - lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, - CollisionType::NO_COLLISION}; - debug_data.bounds_candidates.push_back(ref_points.back().bounds); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return; -} + // calculate trajectory points from optimization result + std::vector traj_points; + for (size_t i = 0; i < N_ref; ++i) { + auto & ref_point = ref_points.at(i); -void MPTOptimizer::calcVehicleBounds( - std::vector & ref_points, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); + const double lat_error = X(i * D_x); + const double yaw_error = X(i * D_x + 1); - if (ref_points.size() == 1) { - for ([[maybe_unused]] const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { - ref_points.at(0).vehicle_bounds.push_back(ref_points.at(0).bounds); - ref_points.at(0).beta.push_back(0.0); + // validate optimization result + if (mpt_param_.enable_optimization_validation) { + if ( + mpt_param_.max_validation_lat_error < std::abs(lat_error) || + mpt_param_.max_validation_yaw_error < std::abs(yaw_error)) { + return std::nullopt; + } } - return; - } - SplineInterpolationPoints2d ref_points_spline_interpolation( - points_utils::convertToPoints(ref_points)); - - for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { - const auto & ref_point = ref_points.at(p_idx); - ref_points.at(p_idx).vehicle_bounds.clear(); - ref_points.at(p_idx).beta.clear(); + // memorize optimization result (optimized_kinematic_state and optimized_input) + ref_point.optimized_kinematic_state = KinematicState{lat_error, yaw_error}; + if (i == N_ref - 1) { + ref_point.optimized_input = 0.0; + } else { + ref_point.optimized_input = steer_angles(D_x + i * D_u); + } - for (const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { - geometry_msgs::msg::Pose avoid_traj_pose; - avoid_traj_pose.position = - ref_points_spline_interpolation.getSplineInterpolatedPoint(p_idx, d); - const double vehicle_bounds_pose_yaw = - ref_points_spline_interpolation.getSplineInterpolatedYaw(p_idx, d); - avoid_traj_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(vehicle_bounds_pose_yaw); - - const double avoid_yaw = std::atan2( - avoid_traj_pose.position.y - ref_point.p.y, avoid_traj_pose.position.x - ref_point.p.x); - const double beta = ref_point.yaw - vehicle_bounds_pose_yaw; - ref_points.at(p_idx).beta.push_back(beta); + std::vector tmp_slack_variables; + for (size_t j = 0; j < N_slack; ++j) { + tmp_slack_variables.push_back(slack_variables(i * N_slack + j)); + } + ref_point.slack_variables = tmp_slack_variables; - const double offset_y = -tier4_autoware_utils::calcDistance2d(ref_point, avoid_traj_pose) * - std::sin(avoid_yaw - vehicle_bounds_pose_yaw); + // update pose and velocity + TrajectoryPoint traj_point; + traj_point.pose = ref_point.offsetDeviation(lat_error, yaw_error); + traj_point.longitudinal_velocity_mps = ref_point.longitudinal_velocity_mps; - const auto vehicle_bounds_pose = - tier4_autoware_utils::calcOffsetPose(avoid_traj_pose, 0.0, offset_y, 0.0); + traj_points.push_back(traj_point); + } - // interpolate bounds - const double avoid_s = ref_points_spline_interpolation.getAccumulatedLength(p_idx) + d; - for (size_t cp_idx = 0; cp_idx < ref_points.size(); ++cp_idx) { - const double current_s = ref_points_spline_interpolation.getAccumulatedLength(cp_idx); - if (avoid_s <= current_s) { - double prev_avoid_idx; - if (cp_idx == 0) { - prev_avoid_idx = cp_idx; - } else { - prev_avoid_idx = cp_idx - 1; - } + time_keeper_ptr_->toc(__func__, " "); + return traj_points; +} - const double prev_s = - ref_points_spline_interpolation.getAccumulatedLength(prev_avoid_idx); - const double next_s = - ref_points_spline_interpolation.getAccumulatedLength(prev_avoid_idx + 1); - const double ratio = (avoid_s - prev_s) / (next_s - prev_s); +void MPTOptimizer::publishDebugTrajectories( + const std_msgs::msg::Header & header, const std::vector & ref_points, + const std::vector & mpt_traj_points) const +{ + // reference points + const auto ref_traj = trajectory_utils::createTrajectory( + header, trajectory_utils::convertToTrajectoryPoints(ref_points)); + debug_ref_traj_pub_->publish(ref_traj); + + // fixed reference points + const auto fixed_traj_points = extractFixedPoints(ref_points); + const auto fixed_traj = trajectory_utils::createTrajectory(header, fixed_traj_points); + debug_fixed_traj_pub_->publish(fixed_traj); + + // mpt points + const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points); + debug_mpt_traj_pub_->publish(mpt_traj); +} - const auto prev_bounds = ref_points.at(prev_avoid_idx).bounds; - const auto next_bounds = ref_points.at(prev_avoid_idx + 1).bounds; +std::vector MPTOptimizer::extractFixedPoints( + const std::vector & ref_points) const +{ + std::vector fixed_traj_points; + for (const auto & ref_point : ref_points) { + if (ref_point.fixed_kinematic_state) { + TrajectoryPoint fixed_traj_point; + fixed_traj_point.pose = ref_point.offsetDeviation( + ref_point.fixed_kinematic_state->lat, ref_point.fixed_kinematic_state->yaw); + fixed_traj_points.push_back(fixed_traj_point); + } + } - auto bounds = Bounds::lerp(prev_bounds, next_bounds, ratio); - bounds.translate(offset_y); + return fixed_traj_points; +} - ref_points.at(p_idx).vehicle_bounds.push_back(bounds); - break; - } +double MPTOptimizer::getTrajectoryLength() const +{ + const double forward_traj_length = mpt_param_.num_points * mpt_param_.delta_arc_length; + const double backward_traj_length = traj_param_.output_backward_traj_length; + return forward_traj_length + backward_traj_length; +} - if (cp_idx == ref_points.size() - 1) { - ref_points.at(p_idx).vehicle_bounds.push_back(ref_points.back().bounds); - } - } +int MPTOptimizer::getNumberOfPoints() const { return mpt_param_.num_points; } - ref_points.at(p_idx).vehicle_bounds_poses.push_back(vehicle_bounds_pose); +size_t MPTOptimizer::getNumberOfSlackVariables() const +{ + if (mpt_param_.soft_constraint) { + if (mpt_param_.l_inf_norm) { + return 1; } + return vehicle_circle_longitudinal_offsets_.size(); } + return 0; +} - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; +std::optional MPTOptimizer::calcNormalizedAvoidanceCost( + const ReferencePoint & ref_point) const +{ + const double negative_avoidance_cost = std::min( + -ref_point.bounds.lower_bound - mpt_param_.avoidance_cost_margin, + ref_point.bounds.upper_bound - mpt_param_.avoidance_cost_margin); + if (0 <= negative_avoidance_cost) { + return {}; + } + return std::clamp(-negative_avoidance_cost / mpt_param_.max_avoidance_cost, 0.0, 1.0); } +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index bee261ef532e0..e387cbbbc2b1e 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -15,581 +15,117 @@ #include "obstacle_avoidance_planner/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/motion_utils.hpp" -#include "obstacle_avoidance_planner/utils/debug_utils.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" +#include "obstacle_avoidance_planner/debug_marker.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" -#include "tf2/utils.h" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "std_msgs/msg/bool.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include #include #include -#include -#include -#include - -namespace -{ -std::tuple, std::vector> calcVehicleCirclesInfo( - const VehicleParam & vehicle_param, const size_t circle_num, const double rear_radius_ratio, - const double front_radius_ratio) -{ - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - { // 1st circle (rear) - const double radius = (vehicle_param.width / 2.0 + lateral_offset) * rear_radius_ratio; - - longitudinal_offsets.push_back(-vehicle_param.rear_overhang); - radiuses.push_back(radius); - } - - { // 2nd circle (front) - const double radius = - (std::hypot( - vehicle_param.length / static_cast(circle_num) / 2.0, - (vehicle_param.width / 2.0 + lateral_offset)) * - front_radius_ratio); - - const double unit_lon_length = vehicle_param.length / static_cast(circle_num); - const double longitudinal_offset = - unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_param.rear_overhang; - - longitudinal_offsets.push_back(longitudinal_offset); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} - -std::tuple, std::vector> calcVehicleCirclesInfo( - const VehicleParam & vehicle_param, const size_t circle_num, const double radius_ratio) -{ - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - const double radius = std::hypot( - vehicle_param.length / static_cast(circle_num) / 2.0, - (vehicle_param.width / 2.0 + lateral_offset)) * - radius_ratio; - const double unit_lon_length = vehicle_param.length / static_cast(circle_num); - - for (size_t i = 0; i < circle_num; ++i) { - longitudinal_offsets.push_back( - unit_lon_length / 2.0 + unit_lon_length * i - vehicle_param.rear_overhang); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} - -std::tuple, std::vector> calcVehicleCirclesInfo( - const VehicleParam & vehicle_param, size_t circle_num) -{ - circle_num = std::max(circle_num, static_cast(2)); - - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - const double radius = (vehicle_param.width / 2.0 + lateral_offset); - const double unit_lon_length = vehicle_param.length / static_cast(circle_num - 1); - - for (size_t i = 0; i < circle_num; ++i) { - longitudinal_offsets.push_back(unit_lon_length * i - vehicle_param.rear_overhang); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} -std::tuple, std::vector> calcVehicleCirclesInfoByBicycleModel( - const VehicleParam & vehicle_param, const size_t circle_num, const double rear_radius_ratio, - const double front_radius_ratio) +namespace obstacle_avoidance_planner { - std::vector longitudinal_offsets; - std::vector radiuses; - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - { // 1st circle (rear wheel) - const double radius = (vehicle_param.width / 2.0 + lateral_offset) * rear_radius_ratio; - longitudinal_offsets.push_back(0.0); - radiuses.push_back(radius); - } - - { // 2nd circle (front wheel) - const double radius = std::hypot( - vehicle_param.length / static_cast(circle_num) / 2.0, - (vehicle_param.width / 2.0 + lateral_offset)) * - front_radius_ratio; - - const double unit_lon_length = vehicle_param.length / static_cast(circle_num); - const double longitudinal_offset = - unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_param.rear_overhang; - - longitudinal_offsets.push_back(longitudinal_offset); - radiuses.push_back(radius); - } - - return {radiuses, longitudinal_offsets}; -} - -[[maybe_unused]] void fillYawInTrajectoryPoint(std::vector & traj_points) +namespace { - std::vector points; - for (const auto & traj_point : traj_points) { - points.push_back(traj_point.pose.position); - } - const auto yaw_vec = interpolation::splineYawFromPoints(points); - - for (size_t i = 0; i < traj_points.size(); ++i) { - traj_points.at(i).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i)); - } -} - template -[[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, - const double yaw_threshold) -{ - const auto nearest_idx_optional = - motion_utils::findNearestIndex(points, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points, pose.position); -} - -template <> -[[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) +std::vector concatVectors(const std::vector & prev_vector, const std::vector & next_vector) { - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); - - return findNearestIndexWithSoftYawConstraints( - points_with_yaw, pose, dist_threshold, yaw_threshold); + std::vector concatenated_vector; + concatenated_vector.insert(concatenated_vector.end(), prev_vector.begin(), prev_vector.end()); + concatenated_vector.insert(concatenated_vector.end(), next_vector.begin(), next_vector.end()); + return concatenated_vector; } -template -[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, - const double yaw_threshold) +StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) { - const auto nearest_idx_optional = - motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestSegmentIndex(points, pose.position); + StringStamped msg; + msg.stamp = now; + msg.data = data; + return msg; } -template <> -[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) +void setZeroVelocityAfterStopPoint(std::vector & traj_points) { - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); - - return findNearestSegmentIndexWithSoftYawConstraints( - points_with_yaw, pose, dist_threshold, yaw_threshold); -} - -template -size_t searchExtendedZeroVelocityIndex( - const std::vector & fine_points, const std::vector & vel_points, - const double yaw_threshold) -{ - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points); - const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; - return findNearestIndexWithSoftYawConstraints( - fine_points, vel_points.at(zero_vel_idx).pose, std::numeric_limits::max(), - yaw_threshold); + const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); + if (opt_zero_vel_idx) { + for (size_t i = opt_zero_vel_idx.get(); i < traj_points.size(); ++i) { + traj_points.at(i).longitudinal_velocity_mps = 0.0; + } + } } -Trajectory createTrajectory( - const std::vector & traj_points, const std_msgs::msg::Header & header) +bool hasZeroVelocity(const TrajectoryPoint & traj_point) { - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - -std::vector resampleTrajectoryPoints( - const std::vector & traj_points, const double interval) -{ - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); - - // convert Trajectory to std::vector - std::vector resampled_traj_points; - for (const auto & point : resampled_traj.points) { - resampled_traj_points.push_back(point); - } - - return resampled_traj_points; + constexpr double zero_vel = 0.0001; + return std::abs(traj_point.longitudinal_velocity_mps) < zero_vel; } } // namespace ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) : Node("obstacle_avoidance_planner", node_options), - logger_ros_clock_(RCL_ROS_TIME), - eb_solved_count_(0) + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + debug_data_ptr_(std::make_shared()), + time_keeper_ptr_(std::make_shared()) { - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); - - // qos - rclcpp::QoS durable_qos{1}; - durable_qos.transient_local(); - - // publisher to other nodes + // interface publisher traj_pub_ = create_publisher("~/output/path", 1); + virtual_wall_pub_ = create_publisher("~/virtual_wall", 1); - // debug publisher - debug_eb_traj_pub_ = create_publisher("~/debug/eb_trajectory", durable_qos); - debug_extended_fixed_traj_pub_ = create_publisher("~/debug/extended_fixed_traj", 1); - debug_extended_non_fixed_traj_pub_ = - create_publisher("~/debug/extended_non_fixed_traj", 1); - debug_mpt_fixed_traj_pub_ = create_publisher("~/debug/mpt_fixed_traj", 1); - debug_mpt_ref_traj_pub_ = create_publisher("~/debug/mpt_ref_traj", 1); - debug_mpt_traj_pub_ = create_publisher("~/debug/mpt_traj", 1); - debug_markers_pub_ = - create_publisher("~/debug/marker", durable_qos); - debug_wall_markers_pub_ = - create_publisher("~/debug/virtual_wall", durable_qos); - debug_clearance_map_pub_ = create_publisher("~/debug/clearance_map", durable_qos); - debug_object_clearance_map_pub_ = - create_publisher("~/debug/object_clearance_map", durable_qos); - debug_area_with_objects_pub_ = - create_publisher("~/debug/area_with_objects", durable_qos); - debug_msg_pub_ = - create_publisher("~/debug/calculation_time", 1); - - // subscriber + // interface subscriber path_sub_ = create_subscription( - "~/input/path", rclcpp::QoS{1}, - std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); + "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "/localization/kinematic_state", rclcpp::QoS{1}, - std::bind(&ObstacleAvoidancePlanner::onOdometry, this, std::placeholders::_1)); - objects_sub_ = create_subscription( - "~/input/objects", rclcpp::QoS{10}, - std::bind(&ObstacleAvoidancePlanner::onObjects, this, std::placeholders::_1)); - is_avoidance_sub_ = create_subscription( - "/planning/scenario_planning/lane_driving/obstacle_avoidance_approval", rclcpp::QoS{10}, - std::bind(&ObstacleAvoidancePlanner::onEnableAvoidance, this, std::placeholders::_1)); - - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - { // vehicle param - vehicle_param_ = VehicleParam{}; - vehicle_param_.width = vehicle_info.vehicle_width_m; - vehicle_param_.length = vehicle_info.vehicle_length_m; - vehicle_param_.wheelbase = vehicle_info.wheel_base_m; - vehicle_param_.rear_overhang = vehicle_info.rear_overhang_m; - vehicle_param_.front_overhang = vehicle_info.front_overhang_m; - vehicle_param_.right_overhang = vehicle_info.right_overhang_m; - vehicle_param_.left_overhang = vehicle_info.left_overhang_m; - vehicle_param_.wheel_tread = vehicle_info.wheel_tread_m; - } - - { // option parameter - is_publishing_debug_visualization_marker_ = - declare_parameter("option.is_publishing_debug_visualization_marker"); - is_publishing_clearance_map_ = declare_parameter("option.is_publishing_clearance_map"); - is_publishing_object_clearance_map_ = - declare_parameter("option.is_publishing_object_clearance_map"); - is_publishing_area_with_objects_ = - declare_parameter("option.is_publishing_area_with_objects"); - - is_showing_debug_info_ = declare_parameter("option.is_showing_debug_info"); - is_showing_calculation_time_ = declare_parameter("option.is_showing_calculation_time"); - is_stopping_if_outside_drivable_area_ = - declare_parameter("option.is_stopping_if_outside_drivable_area"); - - enable_avoidance_ = declare_parameter("option.enable_avoidance"); - enable_pre_smoothing_ = declare_parameter("option.enable_pre_smoothing"); - skip_optimization_ = declare_parameter("option.skip_optimization"); - reset_prev_optimization_ = declare_parameter("option.reset_prev_optimization"); - is_considering_footprint_edges_ = - declare_parameter("option.is_considering_footprint_edges"); - - vehicle_stop_margin_outside_drivable_area_ = - declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); - } - - { // trajectory parameter - traj_param_ = TrajectoryParam{}; - - // common - traj_param_.num_sampling_points = declare_parameter("common.num_sampling_points"); - traj_param_.trajectory_length = declare_parameter("common.trajectory_length"); - traj_param_.forward_fixing_min_distance = - declare_parameter("common.forward_fixing_min_distance"); - traj_param_.forward_fixing_min_time = - declare_parameter("common.forward_fixing_min_time"); - traj_param_.backward_fixing_distance = - declare_parameter("common.backward_fixing_distance"); - traj_param_.delta_arc_length_for_trajectory = - declare_parameter("common.delta_arc_length_for_trajectory"); - - traj_param_.delta_dist_threshold_for_closest_point = - declare_parameter("common.delta_dist_threshold_for_closest_point"); - traj_param_.delta_yaw_threshold_for_closest_point = - declare_parameter("common.delta_yaw_threshold_for_closest_point"); - traj_param_.delta_yaw_threshold_for_straight = - declare_parameter("common.delta_yaw_threshold_for_straight"); - - traj_param_.num_fix_points_for_extending = - declare_parameter("common.num_fix_points_for_extending"); - traj_param_.max_dist_for_extending_end_point = - declare_parameter("common.max_dist_for_extending_end_point"); - traj_param_.non_fixed_trajectory_length = - declare_parameter("common.non_fixed_trajectory_length"); - - traj_param_.enable_clipping_fixed_traj = - declare_parameter("common.enable_clipping_fixed_traj"); - - // object - traj_param_.max_avoiding_ego_velocity_ms = - declare_parameter("object.max_avoiding_ego_velocity_ms"); - traj_param_.max_avoiding_objects_velocity_ms = - declare_parameter("object.max_avoiding_objects_velocity_ms"); - traj_param_.is_avoiding_unknown = - declare_parameter("object.avoiding_object_type.unknown", true); - traj_param_.is_avoiding_car = declare_parameter("object.avoiding_object_type.car", true); - traj_param_.is_avoiding_truck = - declare_parameter("object.avoiding_object_type.truck", true); - traj_param_.is_avoiding_bus = declare_parameter("object.avoiding_object_type.bus", true); - traj_param_.is_avoiding_bicycle = - declare_parameter("object.avoiding_object_type.bicycle", true); - traj_param_.is_avoiding_motorbike = - declare_parameter("object.avoiding_object_type.motorbike", true); - traj_param_.is_avoiding_pedestrian = - declare_parameter("object.avoiding_object_type.pedestrian", true); - traj_param_.is_avoiding_animal = - declare_parameter("object.avoiding_object_type.animal", true); - - // ego nearest search - traj_param_.ego_nearest_dist_threshold = - declare_parameter("ego_nearest_dist_threshold"); - traj_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - } - - { // elastic band parameter - eb_param_ = EBParam{}; - - // common - eb_param_.num_joint_buffer_points = - declare_parameter("advanced.eb.common.num_joint_buffer_points"); - eb_param_.num_offset_for_begin_idx = - declare_parameter("advanced.eb.common.num_offset_for_begin_idx"); - eb_param_.delta_arc_length_for_eb = - declare_parameter("advanced.eb.common.delta_arc_length_for_eb"); - eb_param_.num_sampling_points_for_eb = - declare_parameter("advanced.eb.common.num_sampling_points_for_eb"); - - // clearance - eb_param_.clearance_for_straight_line = - declare_parameter("advanced.eb.clearance.clearance_for_straight_line"); - eb_param_.clearance_for_joint = - declare_parameter("advanced.eb.clearance.clearance_for_joint"); - eb_param_.clearance_for_only_smoothing = - declare_parameter("advanced.eb.clearance.clearance_for_only_smoothing"); - - // qp - eb_param_.qp_param.max_iteration = declare_parameter("advanced.eb.qp.max_iteration"); - eb_param_.qp_param.eps_abs = declare_parameter("advanced.eb.qp.eps_abs"); - eb_param_.qp_param.eps_rel = declare_parameter("advanced.eb.qp.eps_rel"); - - // other - eb_param_.clearance_for_fixing = 0.0; - } - - { // mpt param - mpt_param_ = MPTParam{}; - - // option - // TODO(murooka) implement plan_from_ego - mpt_param_.plan_from_ego = declare_parameter("mpt.option.plan_from_ego"); - mpt_param_.max_plan_from_ego_length = - declare_parameter("mpt.option.max_plan_from_ego_length"); - mpt_param_.steer_limit_constraint = - declare_parameter("mpt.option.steer_limit_constraint"); - mpt_param_.fix_points_around_ego = declare_parameter("mpt.option.fix_points_around_ego"); - mpt_param_.enable_warm_start = declare_parameter("mpt.option.enable_warm_start"); - mpt_param_.enable_manual_warm_start = - declare_parameter("mpt.option.enable_manual_warm_start"); - mpt_visualize_sampling_num_ = declare_parameter("mpt.option.visualize_sampling_num"); - mpt_param_.is_fixed_point_single = declare_parameter("mpt.option.is_fixed_point_single"); - - // common - mpt_param_.num_curvature_sampling_points = - declare_parameter("mpt.common.num_curvature_sampling_points"); - - mpt_param_.delta_arc_length_for_mpt_points = - declare_parameter("mpt.common.delta_arc_length_for_mpt_points"); - - // kinematics - mpt_param_.max_steer_rad = vehicle_info.max_steer_angle_rad; - - // By default, optimization_center_offset will be vehicle_info.wheel_base * 0.8 - // The 0.8 scale is adopted as it performed the best. - constexpr double default_wheelbase_ratio = 0.8; - mpt_param_.optimization_center_offset = declare_parameter( - "mpt.kinematics.optimization_center_offset", - vehicle_param_.wheelbase * default_wheelbase_ratio); - - // bounds search - mpt_param_.bounds_search_widths = - declare_parameter>("advanced.mpt.bounds_search_widths"); - - // collision free constraints - mpt_param_.l_inf_norm = - declare_parameter("advanced.mpt.collision_free_constraints.option.l_inf_norm"); - mpt_param_.soft_constraint = - declare_parameter("advanced.mpt.collision_free_constraints.option.soft_constraint"); - mpt_param_.hard_constraint = - declare_parameter("advanced.mpt.collision_free_constraints.option.hard_constraint"); - - // TODO(murooka) implement two-step soft constraint - mpt_param_.two_step_soft_constraint = false; - // mpt_param_.two_step_soft_constraint = - // declare_parameter("advanced.mpt.collision_free_constraints.option.two_step_soft_constraint"); - - { // vehicle_circles - // NOTE: Vehicle shape for collision free constraints is considered as a set of circles - vehicle_circle_method_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.method"); - - if (vehicle_circle_method_ == "uniform_circle") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.num"); - vehicle_circle_radius_ratios_.push_back(declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio")); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front()); - } else if (vehicle_circle_method_ == "fitting_uniform_circle") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num"); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); - } else if (vehicle_circle_method_ == "rear_drive") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.num_for_calculation"); - - vehicle_circle_radius_ratios_.push_back(declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.rear_radius_ratio")); - vehicle_circle_radius_ratios_.push_back(declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.front_radius_ratio")); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front(), vehicle_circle_radius_ratios_.back()); - } else if (vehicle_circle_method_ == "bicycle_model") { - vehicle_circle_num_for_calculation_ = declare_parameter( - "advanced.mpt.collision_free_constraints.vehicle_circles.bicycle_model.num_for_" - "calculation"); - - vehicle_circle_radius_ratios_.push_back( - declare_parameter("advanced.mpt.collision_free_constraints.vehicle_circles." - "bicycle_model.rear_radius_ratio")); - vehicle_circle_radius_ratios_.push_back( - declare_parameter("advanced.mpt.collision_free_constraints.vehicle_circles." - "bicycle_model.front_radius_ratio")); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfoByBicycleModel( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front(), vehicle_circle_radius_ratios_.back()); - } else { - throw std::invalid_argument( - "advanced.mpt.collision_free_constraints.vehicle_circles.num parameter is invalid."); - } - } - - // clearance - mpt_param_.hard_clearance_from_road = - declare_parameter("advanced.mpt.clearance.hard_clearance_from_road"); - mpt_param_.soft_clearance_from_road = - declare_parameter("advanced.mpt.clearance.soft_clearance_from_road"); - mpt_param_.soft_second_clearance_from_road = - declare_parameter("advanced.mpt.clearance.soft_second_clearance_from_road"); - mpt_param_.extra_desired_clearance_from_road = - declare_parameter("advanced.mpt.clearance.extra_desired_clearance_from_road"); - mpt_param_.clearance_from_object = - declare_parameter("advanced.mpt.clearance.clearance_from_object"); - - // weight - mpt_param_.soft_avoidance_weight = - declare_parameter("advanced.mpt.weight.soft_avoidance_weight"); - mpt_param_.soft_second_avoidance_weight = - declare_parameter("advanced.mpt.weight.soft_second_avoidance_weight"); - - mpt_param_.lat_error_weight = declare_parameter("advanced.mpt.weight.lat_error_weight"); - mpt_param_.yaw_error_weight = declare_parameter("advanced.mpt.weight.yaw_error_weight"); - mpt_param_.yaw_error_rate_weight = - declare_parameter("advanced.mpt.weight.yaw_error_rate_weight"); - mpt_param_.steer_input_weight = - declare_parameter("advanced.mpt.weight.steer_input_weight"); - mpt_param_.steer_rate_weight = - declare_parameter("advanced.mpt.weight.steer_rate_weight"); - - mpt_param_.obstacle_avoid_lat_error_weight = - declare_parameter("advanced.mpt.weight.obstacle_avoid_lat_error_weight"); - mpt_param_.obstacle_avoid_yaw_error_weight = - declare_parameter("advanced.mpt.weight.obstacle_avoid_yaw_error_weight"); - mpt_param_.obstacle_avoid_steer_input_weight = - declare_parameter("advanced.mpt.weight.obstacle_avoid_steer_input_weight"); - mpt_param_.near_objects_length = - declare_parameter("advanced.mpt.weight.near_objects_length"); - - mpt_param_.terminal_lat_error_weight = - declare_parameter("advanced.mpt.weight.terminal_lat_error_weight"); - mpt_param_.terminal_yaw_error_weight = - declare_parameter("advanced.mpt.weight.terminal_yaw_error_weight"); - mpt_param_.terminal_path_lat_error_weight = - declare_parameter("advanced.mpt.weight.terminal_path_lat_error_weight"); - mpt_param_.terminal_path_yaw_error_weight = - declare_parameter("advanced.mpt.weight.terminal_path_yaw_error_weight"); - } + "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); - { // replan - max_path_shape_change_dist_for_replan_ = - declare_parameter("replan.max_path_shape_change_dist"); - max_ego_moving_dist_for_replan_ = - declare_parameter("replan.max_ego_moving_dist_for_replan"); - max_delta_time_sec_for_replan_ = - declare_parameter("replan.max_delta_time_sec_for_replan"); - } - - // TODO(murooka) tune this param when avoiding with obstacle_avoidance_planner - traj_param_.center_line_width = vehicle_param_.width; + // debug publisher + debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); + debug_markers_pub_ = create_publisher("~/debug/marker", 1); + debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + + { // parameters + // parameter for option + enable_outside_drivable_area_stop_ = + declare_parameter("option.enable_outside_drivable_area_stop"); + enable_smoothing_ = declare_parameter("option.enable_smoothing"); + enable_skip_optimization_ = declare_parameter("option.enable_skip_optimization"); + enable_reset_prev_optimization_ = + declare_parameter("option.enable_reset_prev_optimization"); + use_footprint_polygon_for_outside_drivable_area_check_ = + declare_parameter("option.use_footprint_polygon_for_outside_drivable_area_check"); + + // parameter for debug marker + enable_pub_debug_marker_ = declare_parameter("option.debug.enable_pub_debug_marker"); + + // parameter for debug info + enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); + time_keeper_ptr_->enable_calculation_time_info = + declare_parameter("option.debug.enable_calculation_time_info"); + + // parameters for ego nearest search + ego_nearest_param_ = EgoNearestParam(this); + + // parameters for trajectory + traj_param_ = TrajectoryParam(this); + } + + // create core algorithm pointers with parameter declaration + replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); + eb_path_smoother_ptr_ = std::make_shared( + this, enable_debug_info_, ego_nearest_param_, traj_param_, time_keeper_ptr_); + mpt_optimizer_ptr_ = std::make_shared( + this, enable_debug_info_, ego_nearest_param_, vehicle_info_, traj_param_, debug_data_ptr_, + time_keeper_ptr_); + + // reset planners + // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been + // initialized. + initializePlanning(); // set parameter callback + // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been + // initialized. set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); - - resetPlanning(); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( @@ -597,286 +133,39 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( { using tier4_autoware_utils::updateParam; - { // option parameter - updateParam( - parameters, "option.is_publishing_debug_visualization_marker", - is_publishing_debug_visualization_marker_); - updateParam( - parameters, "option.is_publishing_clearance_map", is_publishing_clearance_map_); - updateParam( - parameters, "option.is_publishing_object_clearance_map", is_publishing_object_clearance_map_); - updateParam( - parameters, "option.is_publishing_area_with_objects", is_publishing_area_with_objects_); - - updateParam(parameters, "option.is_showing_debug_info", is_showing_debug_info_); - updateParam( - parameters, "option.is_showing_calculation_time", is_showing_calculation_time_); - updateParam( - parameters, "option.is_stopping_if_outside_drivable_area", - is_stopping_if_outside_drivable_area_); - - updateParam(parameters, "option.enable_avoidance", enable_avoidance_); - updateParam(parameters, "option.enable_pre_smoothing", enable_pre_smoothing_); - updateParam(parameters, "option.skip_optimization", skip_optimization_); - updateParam(parameters, "option.reset_prev_optimization", reset_prev_optimization_); - updateParam( - parameters, "option.is_considering_footprint_edges", is_considering_footprint_edges_); - - updateParam( - parameters, "common.vehicle_stop_margin_outside_drivable_area", - vehicle_stop_margin_outside_drivable_area_); - } + // parameters for option + updateParam( + parameters, "option.enable_outside_drivable_area_stop", enable_outside_drivable_area_stop_); + updateParam(parameters, "option.enable_smoothing", enable_smoothing_); + updateParam(parameters, "option.enable_skip_optimization", enable_skip_optimization_); + updateParam( + parameters, "option.enable_reset_prev_optimization", enable_reset_prev_optimization_); + updateParam( + parameters, "option.use_footprint_polygon_for_outside_drivable_area_check", + use_footprint_polygon_for_outside_drivable_area_check_); - { // trajectory parameter - // common - updateParam(parameters, "common.num_sampling_points", traj_param_.num_sampling_points); - updateParam(parameters, "common.trajectory_length", traj_param_.trajectory_length); - updateParam( - parameters, "common.forward_fixing_min_distance", traj_param_.forward_fixing_min_distance); - updateParam( - parameters, "common.forward_fixing_min_time", traj_param_.forward_fixing_min_time); - updateParam( - parameters, "common.backward_fixing_distance", traj_param_.backward_fixing_distance); - updateParam( - parameters, "common.delta_arc_length_for_trajectory", - traj_param_.delta_arc_length_for_trajectory); - - updateParam( - parameters, "common.delta_dist_threshold_for_closest_point", - traj_param_.delta_dist_threshold_for_closest_point); - updateParam( - parameters, "common.delta_yaw_threshold_for_closest_point", - traj_param_.delta_yaw_threshold_for_closest_point); - updateParam( - parameters, "common.delta_yaw_threshold_for_straight", - traj_param_.delta_yaw_threshold_for_straight); - updateParam( - parameters, "common.num_fix_points_for_extending", traj_param_.num_fix_points_for_extending); - updateParam( - parameters, "common.max_dist_for_extending_end_point", - traj_param_.max_dist_for_extending_end_point); - - // object - updateParam( - parameters, "object.max_avoiding_ego_velocity_ms", traj_param_.max_avoiding_ego_velocity_ms); - updateParam( - parameters, "object.max_avoiding_objects_velocity_ms", - traj_param_.max_avoiding_objects_velocity_ms); - updateParam( - parameters, "object.avoiding_object_type.unknown", traj_param_.is_avoiding_unknown); - updateParam(parameters, "object.avoiding_object_type.car", traj_param_.is_avoiding_car); - updateParam( - parameters, "object.avoiding_object_type.truck", traj_param_.is_avoiding_truck); - updateParam(parameters, "object.avoiding_object_type.bus", traj_param_.is_avoiding_bus); - updateParam( - parameters, "object.avoiding_object_type.bicycle", traj_param_.is_avoiding_bicycle); - updateParam( - parameters, "object.avoiding_object_type.motorbike", traj_param_.is_avoiding_motorbike); - updateParam( - parameters, "object.avoiding_object_type.pedestrian", traj_param_.is_avoiding_pedestrian); - updateParam( - parameters, "object.avoiding_object_type.animal", traj_param_.is_avoiding_animal); - } + // parameters for debug marker + updateParam(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_); - { // elastic band parameter - // common - updateParam( - parameters, "advanced.eb.common.num_joint_buffer_points", eb_param_.num_joint_buffer_points); - updateParam( - parameters, "advanced.eb.common.num_offset_for_begin_idx", - eb_param_.num_offset_for_begin_idx); - updateParam( - parameters, "advanced.eb.common.delta_arc_length_for_eb", eb_param_.delta_arc_length_for_eb); - updateParam( - parameters, "advanced.eb.common.num_sampling_points_for_eb", - eb_param_.num_sampling_points_for_eb); - - // clearance - updateParam( - parameters, "advanced.eb.clearance.clearance_for_straight_line", - eb_param_.clearance_for_straight_line); - updateParam( - parameters, "advanced.eb.clearance.clearance_for_joint", eb_param_.clearance_for_joint); - updateParam( - parameters, "advanced.eb.clearance.clearance_for_only_smoothing", - eb_param_.clearance_for_only_smoothing); - - // qp - updateParam(parameters, "advanced.eb.qp.max_iteration", eb_param_.qp_param.max_iteration); - updateParam(parameters, "advanced.eb.qp.eps_abs", eb_param_.qp_param.eps_abs); - updateParam(parameters, "advanced.eb.qp.eps_rel", eb_param_.qp_param.eps_rel); - } + // parameters for debug info + updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); + updateParam( + parameters, "option.debug.enable_calculation_time_info", + time_keeper_ptr_->enable_calculation_time_info); - { // mpt param - // option - updateParam(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego); - updateParam( - parameters, "mpt.option.max_plan_from_ego_length", mpt_param_.max_plan_from_ego_length); - updateParam( - parameters, "mpt.option.steer_limit_constraint", mpt_param_.steer_limit_constraint); - updateParam( - parameters, "mpt.option.fix_points_around_ego", mpt_param_.fix_points_around_ego); - updateParam(parameters, "mpt.option.enable_warm_start", mpt_param_.enable_warm_start); - updateParam( - parameters, "mpt.option.enable_manual_warm_start", mpt_param_.enable_manual_warm_start); - updateParam(parameters, "mpt.option.visualize_sampling_num", mpt_visualize_sampling_num_); - updateParam( - parameters, "mpt.option.option.is_fixed_point_single", mpt_param_.is_fixed_point_single); - - // common - updateParam( - parameters, "mpt.common.num_curvature_sampling_points", - mpt_param_.num_curvature_sampling_points); - - updateParam( - parameters, "mpt.common.delta_arc_length_for_mpt_points", - mpt_param_.delta_arc_length_for_mpt_points); - - // kinematics - updateParam( - parameters, "mpt.kinematics.optimization_center_offset", - mpt_param_.optimization_center_offset); - - // collision_free_constraints - updateParam( - parameters, "advanced.mpt.collision_free_constraints.option.l_inf_norm", - mpt_param_.l_inf_norm); - // updateParam( - // parameters, "advanced.mpt.collision_free_constraints.option.two_step_soft_constraint", - // mpt_param_.two_step_soft_constraint); - updateParam( - parameters, "advanced.mpt.collision_free_constraints.option.soft_constraint", - mpt_param_.soft_constraint); - updateParam( - parameters, "advanced.mpt.collision_free_constraints.option.hard_constraint", - mpt_param_.hard_constraint); - - { // vehicle_circles - // NOTE: Changing method is not supported - // updateParam( - // parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.method", - // vehicle_circle_method_); - - if (vehicle_circle_method_ == "uniform_circle") { - updateParam( - parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.num", - vehicle_circle_num_for_calculation_); - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio", - vehicle_circle_radius_ratios_.front()); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front()); - } else if (vehicle_circle_method_ == "fitting_uniform_circle") { - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num", - vehicle_circle_num_for_calculation_); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); - } else if (vehicle_circle_method_ == "rear_drive") { - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.num_for_calculation", - vehicle_circle_num_for_calculation_); - - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.rear_radius_ratio", - vehicle_circle_radius_ratios_.front()); - - updateParam( - parameters, - "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.front_radius_ratio", - vehicle_circle_radius_ratios_.back()); - - std::tie( - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = - calcVehicleCirclesInfo( - vehicle_param_, vehicle_circle_num_for_calculation_, - vehicle_circle_radius_ratios_.front(), vehicle_circle_radius_ratios_.back()); - } else { - throw std::invalid_argument( - "advanced.mpt.collision_free_constraints.vehicle_circles.num parameter is invalid."); - } - } + // parameters for ego nearest search + ego_nearest_param_.onParam(parameters); - // clearance - updateParam( - parameters, "advanced.mpt.clearance.hard_clearance_from_road", - mpt_param_.hard_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.soft_clearance_from_road", - mpt_param_.soft_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.soft_second_clearance_from_road", - mpt_param_.soft_second_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.extra_desired_clearance_from_road", - mpt_param_.extra_desired_clearance_from_road); - updateParam( - parameters, "advanced.mpt.clearance.clearance_from_object", mpt_param_.clearance_from_object); - - // weight - updateParam( - parameters, "advanced.mpt.weight.soft_avoidance_weight", mpt_param_.soft_avoidance_weight); - updateParam( - parameters, "advanced.mpt.weight.soft_second_avoidance_weight", - mpt_param_.soft_second_avoidance_weight); - - updateParam( - parameters, "advanced.mpt.weight.lat_error_weight", mpt_param_.lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.yaw_error_weight", mpt_param_.yaw_error_weight); - updateParam( - parameters, "advanced.mpt.weight.yaw_error_rate_weight", mpt_param_.yaw_error_rate_weight); - updateParam( - parameters, "advanced.mpt.weight.steer_input_weight", mpt_param_.steer_input_weight); - updateParam( - parameters, "advanced.mpt.weight.steer_rate_weight", mpt_param_.steer_rate_weight); - - updateParam( - parameters, "advanced.mpt.weight.obstacle_avoid_lat_error_weight", - mpt_param_.obstacle_avoid_lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.obstacle_avoid_yaw_error_weight", - mpt_param_.obstacle_avoid_yaw_error_weight); - updateParam( - parameters, "advanced.mpt.weight.obstacle_avoid_steer_input_weight", - mpt_param_.obstacle_avoid_steer_input_weight); - updateParam( - parameters, "advanced.mpt.weight.near_objects_length", mpt_param_.near_objects_length); - - updateParam( - parameters, "advanced.mpt.weight.terminal_lat_error_weight", - mpt_param_.terminal_lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.terminal_yaw_error_weight", - mpt_param_.terminal_yaw_error_weight); - updateParam( - parameters, "advanced.mpt.weight.terminal_path_lat_error_weight", - mpt_param_.terminal_path_lat_error_weight); - updateParam( - parameters, "advanced.mpt.weight.terminal_path_yaw_error_weight", - mpt_param_.terminal_path_yaw_error_weight); - } + // parameters for trajectory + traj_param_.onParam(parameters); - { // replan - updateParam( - parameters, "replan.max_path_shape_change_dist", max_path_shape_change_dist_for_replan_); - updateParam( - parameters, "replan.max_ego_moving_dist_for_replan", max_ego_moving_dist_for_replan_); - updateParam( - parameters, "replan.max_delta_time_sec_for_replan", max_delta_time_sec_for_replan_); - } + // parameters for core algorithms + replan_checker_ptr_->onParam(parameters); + eb_path_smoother_ptr_->onParam(parameters); + mpt_optimizer_ptr_->onParam(parameters); - resetPlanning(); + // reset planners + initializePlanning(); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -884,809 +173,394 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( return result; } -void ObstacleAvoidancePlanner::onOdometry(const Odometry::SharedPtr msg) +void ObstacleAvoidancePlanner::initializePlanning() { - current_odometry_ptr_ = std::make_unique(*msg); -} + RCLCPP_INFO(get_logger(), "Initialize planning"); -void ObstacleAvoidancePlanner::onObjects(const PredictedObjects::SharedPtr msg) -{ - objects_ptr_ = std::make_unique(*msg); -} + eb_path_smoother_ptr_->initialize(enable_debug_info_, traj_param_); + mpt_optimizer_ptr_->initialize(enable_debug_info_, traj_param_); -void ObstacleAvoidancePlanner::onEnableAvoidance( - const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr msg) -{ - enable_avoidance_ = msg->enable_avoidance; + resetPreviousData(); } -void ObstacleAvoidancePlanner::resetPlanning() +void ObstacleAvoidancePlanner::resetPreviousData() { - RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - - eb_path_optimizer_ptr_ = std::make_unique( - is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); + eb_path_smoother_ptr_->resetPreviousData(); + mpt_optimizer_ptr_->resetPreviousData(); - mpt_optimizer_ptr_ = - std::make_unique(is_showing_debug_info_, traj_param_, vehicle_param_, mpt_param_); - - prev_path_points_ptr_ = nullptr; - resetPrevOptimization(); -} - -void ObstacleAvoidancePlanner::resetPrevOptimization() -{ - prev_optimal_trajs_ptr_ = nullptr; - eb_solved_count_ = 0; + prev_optimized_traj_points_ptr_ = nullptr; } void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) { - stop_watch_.tic(__func__); + time_keeper_ptr_->init(); + time_keeper_ptr_->tic(__func__); - if (path_ptr->points.empty() || !current_odometry_ptr_ || !objects_ptr_) { + // check if data is ready and valid + if (!isDataReady(*path_ptr, *get_clock())) { return; } - if (path_ptr->left_bound.empty() || path_ptr->right_bound.empty()) { - std::cerr << "Right or left bound is empty" << std::endl; - } + // 0. return if path is backward + // TODO(murooka): support backward path + const auto is_driving_forward = driving_direction_checker_.isDrivingForward(path_ptr->points); + if (!is_driving_forward) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Backward path is NOT supported. Just converting path to trajectory"); - // create planner data - PlannerData planner_data; - planner_data.path = *path_ptr; - planner_data.ego_pose = current_odometry_ptr_->pose.pose; - planner_data.ego_vel = current_odometry_ptr_->twist.twist.linear.x; - planner_data.objects = objects_ptr_->objects; - - debug_data_ = DebugData(); - debug_data_.init( - is_showing_calculation_time_, mpt_visualize_sampling_num_, planner_data.ego_pose, - mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets); - - const auto output_traj_msg = generateTrajectory(planner_data); - - // publish debug data - publishDebugDataInMain(*path_ptr); - - { // print and publish debug msg - debug_data_.msg_stream << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n" - << "========================================"; - tier4_debug_msgs::msg::StringStamped debug_msg_msg; - debug_msg_msg.stamp = get_clock()->now(); - debug_msg_msg.data = debug_data_.msg_stream.getString(); - debug_msg_pub_->publish(debug_msg_msg); + const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); + const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points); + traj_pub_->publish(output_traj_msg); + return; } - // make previous variables - prev_path_points_ptr_ = std::make_unique>(path_ptr->points); - prev_ego_pose_ptr_ = std::make_unique(planner_data.ego_pose); + // 1. create planner data + const auto planner_data = createPlannerData(*path_ptr); - traj_pub_->publish(output_traj_msg); -} + // 2. generate optimized trajectory + const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); -Trajectory ObstacleAvoidancePlanner::generateTrajectory(const PlannerData & planner_data) -{ - const auto & p = planner_data; + // 3. extend trajectory to connect the optimized trajectory and the following path smoothly + auto full_traj_points = extendTrajectory(planner_data.traj_points, optimized_traj_points); - // TODO(someone): support backward path - const auto is_driving_forward = motion_utils::isDrivingForward(p.path.points); - is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; - if (!is_driving_forward_) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, - "[ObstacleAvoidancePlanner] Backward path is NOT supported. Just converting path to " - "trajectory"); + // 4. set zero velocity after stop point + setZeroVelocityAfterStopPoint(full_traj_points); - const auto traj_points = points_utils::convertToTrajectoryPoints(p.path.points); - return createTrajectory(traj_points, p.path.header); - } + // 5. publish debug data + publishDebugData(planner_data.header); - // generate optimized trajectory - const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); - // generate post processed trajectory - const auto post_processed_traj_points = - generatePostProcessedTrajectory(p.path.points, optimized_traj_points, planner_data); + time_keeper_ptr_->toc(__func__, ""); + *time_keeper_ptr_ << "========================================"; + time_keeper_ptr_->endLine(); + + // publish calculation_time + // NOTE: This function must be called after measuring onPath calculation time + const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); + debug_calculation_time_pub_->publish(calculation_time_msg); - // convert to output msg type - return createTrajectory(post_processed_traj_points, p.path.header); + const auto output_traj_msg = + trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + traj_pub_->publish(output_traj_msg); } -std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( - const PlannerData & planner_data) +bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const { - stop_watch_.tic(__func__); - - if (reset_prev_optimization_) { - resetPrevOptimization(); + if (!ego_state_ptr_) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); + return false; } - const auto & path = planner_data.path; - - // return prev trajectory if replan is not required - if (!checkReplan(planner_data)) { - if (prev_optimal_trajs_ptr_) { - return prev_optimal_trajs_ptr_->model_predictive_trajectory; - } - - return points_utils::convertToTrajectoryPoints(path.points); + if (path.points.size() < 2) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); + return false; } - prev_replanned_time_ptr_ = std::make_unique(this->now()); - - // calculate trajectory with EB and MPT - auto optimal_trajs = optimizeTrajectory(planner_data); - // calculate velocity - // NOTE: Velocity is not considered in optimization. - calcVelocity(path.points, optimal_trajs.model_predictive_trajectory); - - // insert 0 velocity when trajectory is over drivable area - if (is_stopping_if_outside_drivable_area_) { - insertZeroVelocityOutsideDrivableArea(planner_data, optimal_trajs.model_predictive_trajectory); + if (path.left_bound.empty() || path.right_bound.empty()) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), clock, 5000, "Left or right bound in path is empty."); + return false; } - publishDebugDataInOptimization(planner_data, optimal_trajs.model_predictive_trajectory); + return true; +} - // make previous trajectories - prev_optimal_trajs_ptr_ = - std::make_unique(makePrevTrajectories(path.points, optimal_trajs, planner_data)); +PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const +{ + // create planner data + PlannerData planner_data; + planner_data.header = path.header; + planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); + planner_data.left_bound = path.left_bound; + planner_data.right_bound = path.right_bound; + planner_data.ego_pose = ego_state_ptr_->pose.pose; + planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return optimal_trajs.model_predictive_trajectory; + debug_data_ptr_->ego_pose = planner_data.ego_pose; + return planner_data; } -// check if optimization is required or not. -// NOTE: previous trajectories information will be reset as well in some cases. -bool ObstacleAvoidancePlanner::checkReplan(const PlannerData & planner_data) +std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( + const PlannerData & planner_data) { - const auto & p = planner_data; - - if ( - !prev_ego_pose_ptr_ || !prev_replanned_time_ptr_ || !prev_path_points_ptr_ || - !prev_optimal_trajs_ptr_) { - return true; - } + time_keeper_ptr_->tic(__func__); - if (prev_optimal_trajs_ptr_->model_predictive_trajectory.empty()) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since previous optimized trajectory is empty."); - resetPrevOptimization(); - return true; - } + const auto & input_traj_points = planner_data.traj_points; - if (isPathShapeChanged(p)) { - RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path shape was changed."); - resetPrevOptimization(); - return true; - } + // 1. calculate trajectory with EB and MPT + // NOTE: This function may return previously optimized trajectory points. + // Also, velocity on some points will not be updated for a logic purpose. + auto optimized_traj_points = optimizeTrajectory(planner_data); - if (isPathGoalChanged(p)) { - RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed."); - resetPrevOptimization(); - return true; - } + // 2. update velocity + // NOTE: When optimization failed or is skipped, velocity in trajectory points must + // be updated since velocity in input trajectory (path) may change. + applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose); - // For when ego pose is lost or new ego pose is designated in simulation - const double delta_dist = - tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); - if (delta_dist > max_ego_moving_dist_for_replan_) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since current ego pose is far from previous ego pose."); - resetPrevOptimization(); - return true; - } + // 3. insert zero velocity when trajectory is over drivable area + insertZeroVelocityOutsideDrivableArea(planner_data, optimized_traj_points); - // For when ego pose moves far from trajectory - if (!isEgoNearToPrevTrajectory(p.ego_pose)) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since valid nearest trajectory point from ego was not " - "found."); - resetPrevOptimization(); - return true; - } + // 4. publish debug marker + publishDebugMarkerOfOptimization(optimized_traj_points); - const double delta_time_sec = (this->now() - *prev_replanned_time_ptr_).seconds(); - if (delta_time_sec > max_delta_time_sec_for_replan_) { - return true; - } - return false; + time_keeper_ptr_->toc(__func__, " "); + return optimized_traj_points; } -bool ObstacleAvoidancePlanner::isPathShapeChanged(const PlannerData & planner_data) +std::vector ObstacleAvoidancePlanner::optimizeTrajectory( + const PlannerData & planner_data) { - if (!prev_path_points_ptr_) { - return false; - } - + time_keeper_ptr_->tic(__func__); const auto & p = planner_data; - const double max_mpt_length = - traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; - - // truncate prev points from ego pose to fixed end points - const auto prev_begin_idx = findEgoNearestIndex(*prev_path_points_ptr_, p.ego_pose); - const auto truncated_prev_points = - points_utils::clipForwardPoints(*prev_path_points_ptr_, prev_begin_idx, max_mpt_length); - - // truncate points from ego pose to fixed end points - const auto begin_idx = findEgoNearestIndex(p.path.points, p.ego_pose); - const auto truncated_points = - points_utils::clipForwardPoints(p.path.points, begin_idx, max_mpt_length); - - // guard for lateral offset - if (truncated_prev_points.size() < 2 || truncated_points.size() < 2) { - return false; - } - - // calculate lateral deviations between truncated path_points and prev_path_points - for (const auto & prev_point : truncated_prev_points) { - const double dist = - std::abs(motion_utils::calcLateralOffset(truncated_points, prev_point.pose.position)); - if (dist > max_path_shape_change_dist_for_replan_) { - return true; + // 1. check if replan (= optimization) is required + const bool reset_prev_optimization = replan_checker_ptr_->isResetRequired(planner_data); + if (enable_reset_prev_optimization_ || reset_prev_optimization) { + // NOTE: always replan when resetting previous optimization + resetPreviousData(); + } else { + // check replan when not resetting previous optimization + const bool is_replan_required = replan_checker_ptr_->isReplanRequired(now()); + if (!is_replan_required) { + return getPrevOptimizedTrajectory(p.traj_points); } } - return false; -} - -bool ObstacleAvoidancePlanner::isPathGoalChanged(const PlannerData & planner_data) -{ - const auto & p = planner_data; - - if (!prev_path_points_ptr_) { - return false; + if (enable_skip_optimization_) { + return p.traj_points; } - constexpr double min_vel = 1e-3; - if (std::abs(p.ego_vel) > min_vel) { - return false; + // 2. smooth trajectory with elastic band + const auto eb_traj = + enable_smoothing_ ? eb_path_smoother_ptr_->getEBTrajectory(planner_data) : p.traj_points; + if (!eb_traj) { + return getPrevOptimizedTrajectory(p.traj_points); } - // NOTE: Path may be cropped and does not contain the goal. - // Therefore we set a large value to distance threshold. - constexpr double max_goal_moving_dist = 1.0; - const double goal_moving_dist = - tier4_autoware_utils::calcDistance2d(p.path.points.back(), prev_path_points_ptr_->back()); - if (goal_moving_dist < max_goal_moving_dist) { - return false; + // 3. make trajectory kinematically-feasible and collision-free (= inside the drivable area) + // with model predictive trajectory + const auto mpt_traj = mpt_optimizer_ptr_->getModelPredictiveTrajectory(planner_data, *eb_traj); + if (!mpt_traj) { + return getPrevOptimizedTrajectory(p.traj_points); } - return true; + // 4. make prev trajectories + prev_optimized_traj_points_ptr_ = std::make_shared>(*mpt_traj); + + time_keeper_ptr_->toc(__func__, " "); + return *mpt_traj; } -bool ObstacleAvoidancePlanner::isEgoNearToPrevTrajectory(const geometry_msgs::msg::Pose & ego_pose) +std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajectory( + const std::vector & traj_points) const { - const auto & traj_points = prev_optimal_trajs_ptr_->model_predictive_trajectory; - - const auto resampled_traj_points = - resampleTrajectoryPoints(traj_points, traj_param_.delta_arc_length_for_trajectory); - const auto opt_nearest_idx = motion_utils::findNearestIndex( - resampled_traj_points, ego_pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - if (!opt_nearest_idx) { - return false; + if (prev_optimized_traj_points_ptr_) { + return *prev_optimized_traj_points_ptr_; } - return true; + + return traj_points; } -Trajectories ObstacleAvoidancePlanner::optimizeTrajectory(const PlannerData & planner_data) +void ObstacleAvoidancePlanner::applyInputVelocity( + std::vector & output_traj_points, + const std::vector & input_traj_points, + const geometry_msgs::msg::Pose & ego_pose) const { - stop_watch_.tic(__func__); - - const auto & p = planner_data; + time_keeper_ptr_->tic(__func__); - if (skip_optimization_) { - const auto traj = points_utils::convertToTrajectoryPoints(p.path.points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; - } + // crop forward for faster calculation + const auto forward_cropped_input_traj_points = [&]() { + const double optimized_traj_length = mpt_optimizer_ptr_->getTrajectoryLength(); + constexpr double margin_traj_length = 10.0; - // EB: smooth trajectory if enable_pre_smoothing is true - const auto eb_traj = [&]() -> boost::optional> { - if (enable_pre_smoothing_) { - return eb_path_optimizer_ptr_->getEBTrajectory( - p.ego_pose, p.path, prev_optimal_trajs_ptr_, p.ego_vel, debug_data_); - } - return points_utils::convertToTrajectoryPoints(p.path.points); + const size_t ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); + return trajectory_utils::cropForwardPoints( + input_traj_points, ego_pose.position, ego_seg_idx, + optimized_traj_length + margin_traj_length); }(); - if (!eb_traj) { - return getPrevTrajs(p.path.points); - } - // NOTE: Elastic band sometimes diverges with status = "OSQP_SOLVED". - constexpr double max_path_change_diff = 1.0e4; - for (size_t i = 0; i < eb_traj->size(); ++i) { - const auto & eb_pos = eb_traj->at(i).pose.position; - const auto & path_pos = p.path.points.at(std::min(i, p.path.points.size() - 1)).pose.position; + // update velocity + size_t input_traj_start_idx = 0; + for (size_t i = 0; i < output_traj_points.size(); i++) { + // crop backward for efficient calculation + const auto cropped_input_traj_points = std::vector{ + forward_cropped_input_traj_points.begin() + input_traj_start_idx, + forward_cropped_input_traj_points.end()}; - const double diff_x = eb_pos.x - path_pos.x; - const double diff_y = eb_pos.y - path_pos.y; - if (max_path_change_diff < std::abs(diff_x) || max_path_change_diff < std::abs(diff_y)) { - return getPrevTrajs(p.path.points); - } - } + const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( + cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); + input_traj_start_idx = nearest_seg_idx; - // EB has to be solved twice before solving MPT with fixed points - // since the result of EB is likely to change with/without fixing (1st/2nd EB) - // that makes MPT fixing points worse. - if (eb_solved_count_ < 2) { - eb_solved_count_++; - - if (prev_optimal_trajs_ptr_) { - prev_optimal_trajs_ptr_->model_predictive_trajectory.clear(); - prev_optimal_trajs_ptr_->mpt_ref_points.clear(); - } + // calculate velocity with zero order hold + const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps; + output_traj_points.at(i).longitudinal_velocity_mps = velocity; } - // MPT: optimize trajectory to be kinematically feasible and collision free - const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - enable_avoidance_, eb_traj.get(), p.path.points, p.path.left_bound, p.path.right_bound, - prev_optimal_trajs_ptr_, p.ego_pose, p.ego_vel, debug_data_); - if (!mpt_trajs) { - return getPrevTrajs(p.path.points); - } - - // make trajectories, which has all optimized trajectories information - Trajectories trajs; - trajs.smoothed_trajectory = eb_traj.get(); - trajs.mpt_ref_points = mpt_trajs.get().ref_points; - trajs.model_predictive_trajectory = mpt_trajs.get().mpt; - - // debug data - debug_data_.mpt_traj = mpt_trajs.get().mpt; - debug_data_.mpt_ref_traj = points_utils::convertToTrajectoryPoints(mpt_trajs.get().ref_points); - debug_data_.eb_traj = eb_traj.get(); - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return trajs; -} + // insert stop point explicitly + const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + if (stop_idx) { + const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_); -Trajectories ObstacleAvoidancePlanner::getPrevTrajs( - const std::vector & path_points) const -{ - if (prev_optimal_trajs_ptr_) { - return *prev_optimal_trajs_ptr_; + // calculate and insert stop pose on output trajectory + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); } - const auto traj = points_utils::convertToTrajectoryPoints(path_points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; -} - -void ObstacleAvoidancePlanner::calcVelocity( - const std::vector & path_points, std::vector & traj_points) const -{ - for (size_t i = 0; i < traj_points.size(); i++) { - const size_t nearest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( - path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - // add this line not to exceed max index size - const size_t max_idx = std::min(nearest_seg_idx + 1, path_points.size() - 1); - // NOTE: std::max, not std::min, is used here since traj_points' sampling width may be longer - // than path_points' sampling width. A zero velocity point is guaranteed to be inserted in an - // output trajectory in the alignVelocity function - traj_points.at(i).longitudinal_velocity_mps = std::max( - path_points.at(nearest_seg_idx).longitudinal_velocity_mps, - path_points.at(max_idx).longitudinal_velocity_mps); - } + time_keeper_ptr_->toc(__func__, " "); } void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points) + const PlannerData & planner_data, std::vector & optimized_traj_points) const { - if (traj_points.empty()) { + time_keeper_ptr_->tic(__func__); + + if (!enable_outside_drivable_area_stop_) { return; } - stop_watch_.tic(__func__); - - const size_t nearest_idx = findEgoNearestIndex(traj_points, planner_data.ego_pose); - - // NOTE: Some end trajectory points will be ignored to check if outside the drivable area - // since these points might be outside drivable area if only end reference points have high - // curvature. - const size_t end_idx = [&]() { - const size_t enough_long_points_num = - static_cast(traj_param_.num_sampling_points * 0.7); - constexpr size_t end_ignored_points_num = 5; - if (traj_points.size() > enough_long_points_num) { - return std::max(static_cast(0), traj_points.size() - end_ignored_points_num); - } - return traj_points.size(); - }(); - - const auto left_bound = planner_data.path.left_bound; - const auto right_bound = planner_data.path.right_bound; - if (left_bound.empty() || right_bound.empty()) { + if (optimized_traj_points.empty()) { return; } - for (size_t i = nearest_idx; i < end_idx; ++i) { - const auto & traj_point = traj_points.at(i); - - // calculate the first point being outside drivable area - const bool is_outside = drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( - traj_point, left_bound, right_bound, vehicle_param_, is_considering_footprint_edges_); - - // only insert zero velocity to the point with longitudinal offset margin from the first point - // outside drivable area - if (is_outside) { - size_t stop_idx = i; - const auto & op_target_point = motion_utils::calcLongitudinalOffsetPoint( - traj_points, traj_point.pose.position, -1.0 * vehicle_stop_margin_outside_drivable_area_); - - if (op_target_point) { - const auto target_point = op_target_point.get(); - // confirm that target point doesn't overlap with the stop point outside drivable area - const auto dist = tier4_autoware_utils::calcDistance2d(traj_point, target_point); - const double overlap_threshold = 1e-3; - if (dist > overlap_threshold) { - stop_idx = motion_utils::findNearestSegmentIndex(traj_points, target_point); - } - } - - traj_points[stop_idx].longitudinal_velocity_mps = 0.0; - debug_data_.stop_pose_by_drivable_area = traj_points[stop_idx].pose; - - // NOTE: traj_points does not have valid z for efficient calculation of trajectory - if (!planner_data.path.points.empty()) { - const size_t path_idx = motion_utils::findNearestIndex( - planner_data.path.points, traj_points[stop_idx].pose.position); - - debug_data_.stop_pose_by_drivable_area->position.z = - planner_data.path.points.at(path_idx).pose.position.z; + // 1. calculate ego_index nearest to optimized_traj_points + const size_t ego_idx = trajectory_utils::findEgoIndex( + optimized_traj_points, planner_data.ego_pose, ego_nearest_param_); + + // 2. calculate an end point to check being outside the drivable area + // NOTE: Some terminal trajectory points tend to be outside the drivable area when + // they have high curvature. + // Therefore, these points should be ignored to check if being outside the drivable area + constexpr int num_points_ignore_drivable_area = 5; + const int end_idx = std::min( + static_cast(optimized_traj_points.size()) - 1, + mpt_optimizer_ptr_->getNumberOfPoints() - num_points_ignore_drivable_area); + + // 3. assign zero velocity to the first point being outside the drivable area + const auto first_outside_idx = [&]() -> std::optional { + for (size_t i = ego_idx; i < static_cast(end_idx); ++i) { + auto & traj_point = optimized_traj_points.at(i); + + // check if the footprint is outside the drivable area + const bool is_outside = geometry_utils::isOutsideDrivableAreaFromRectangleFootprint( + traj_point.pose, planner_data.left_bound, planner_data.right_bound, vehicle_info_, + use_footprint_polygon_for_outside_drivable_area_check_); + + if (is_outside) { + publishVirtualWall(traj_point.pose); + return i; } - break; } - } - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; -} - -void ObstacleAvoidancePlanner::publishDebugDataInOptimization( - const PlannerData & planner_data, const std::vector & traj_points) -{ - stop_watch_.tic(__func__); - - const auto & p = planner_data; - - { // publish trajectories - const auto debug_eb_traj = createTrajectory(debug_data_.eb_traj, p.path.header); - debug_eb_traj_pub_->publish(debug_eb_traj); - - const auto debug_mpt_fixed_traj = createTrajectory(debug_data_.mpt_fixed_traj, p.path.header); - debug_mpt_fixed_traj_pub_->publish(debug_mpt_fixed_traj); - - const auto debug_mpt_ref_traj = createTrajectory(debug_data_.mpt_ref_traj, p.path.header); - debug_mpt_ref_traj_pub_->publish(debug_mpt_ref_traj); - - const auto debug_mpt_traj = createTrajectory(debug_data_.mpt_traj, p.path.header); - debug_mpt_traj_pub_->publish(debug_mpt_traj); - } + return std::nullopt; + }(); - { // publish markers - if (is_publishing_debug_visualization_marker_) { - stop_watch_.tic("getDebugVisualizationMarker"); - const auto & debug_marker = - debug_utils::getDebugVisualizationMarker(debug_data_, traj_points, vehicle_param_, false); - debug_data_.msg_stream << " getDebugVisualizationMarker:= " - << stop_watch_.toc("getDebugVisualizationMarker") << " [ms]\n"; - - stop_watch_.tic("publishDebugVisualizationMarker"); - debug_markers_pub_->publish(debug_marker); - debug_data_.msg_stream << " publishDebugVisualizationMarker:= " - << stop_watch_.toc("publishDebugVisualizationMarker") << " [ms]\n"; - - stop_watch_.tic("getDebugVisualizationWallMarker"); - const auto & debug_wall_marker = - debug_utils::getDebugVisualizationWallMarker(debug_data_, vehicle_param_); - debug_data_.msg_stream << " getDebugVisualizationWallMarker:= " - << stop_watch_.toc("getDebugVisualizationWallMarker") << " [ms]\n"; - - stop_watch_.tic("publishDebugVisualizationWallMarker"); - debug_wall_markers_pub_->publish(debug_wall_marker); - debug_data_.msg_stream << " publishDebugVisualizationWallMarker:= " - << stop_watch_.toc("publishDebugVisualizationWallMarker") << " [ms]\n"; + if (first_outside_idx) { + for (size_t i = *first_outside_idx; i < optimized_traj_points.size(); ++i) { + optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0; } } - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); } -Trajectories ObstacleAvoidancePlanner::makePrevTrajectories( - const std::vector & path_points, const Trajectories & trajs, - const PlannerData & planner_data) +void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - stop_watch_.tic(__func__); + time_keeper_ptr_->tic(__func__); - const auto post_processed_smoothed_traj = - generatePostProcessedTrajectory(path_points, trajs.smoothed_trajectory, planner_data); + const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); - // TODO(murooka) generatePoseProcessedTrajectory may be too large - Trajectories trajectories; - trajectories.smoothed_trajectory = post_processed_smoothed_traj; - trajectories.mpt_ref_points = trajs.mpt_ref_points; - trajectories.model_predictive_trajectory = trajs.model_predictive_trajectory; - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - - return trajectories; + virtual_wall_pub_->publish(virtual_wall_marker); + time_keeper_ptr_->toc(__func__, " "); } -std::vector ObstacleAvoidancePlanner::generatePostProcessedTrajectory( - const std::vector & path_points, - const std::vector & optimized_traj_points, const PlannerData & planner_data) +void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( + const std::vector & traj_points) const { - stop_watch_.tic(__func__); - - std::vector trajectory_points; - if (path_points.empty()) { - TrajectoryPoint tmp_point; - tmp_point.pose = planner_data.ego_pose; - tmp_point.longitudinal_velocity_mps = 0.0; - trajectory_points.push_back(tmp_point); - return trajectory_points; - } - if (optimized_traj_points.empty()) { - trajectory_points = points_utils::convertToTrajectoryPoints(path_points); - return trajectory_points; + if (!enable_pub_debug_marker_) { + return; } - // calculate extended trajectory that connects to optimized trajectory smoothly - const auto extended_traj_points = getExtendedTrajectory(path_points, optimized_traj_points); + time_keeper_ptr_->tic(__func__); - // concat trajectories - const auto full_traj_points = - points_utils::concatTrajectory(optimized_traj_points, extended_traj_points); + // debug marker + time_keeper_ptr_->tic("getDebugMarker"); + const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_); + time_keeper_ptr_->toc("getDebugMarker", " "); - // NOTE: fine_traj_points has no velocity information - const auto fine_traj_points = generateFineTrajectoryPoints(path_points, full_traj_points); + time_keeper_ptr_->tic("publishDebugMarker"); + debug_markers_pub_->publish(debug_marker); + time_keeper_ptr_->toc("publishDebugMarker", " "); - const auto fine_traj_points_with_vel = - alignVelocity(fine_traj_points, path_points, full_traj_points); - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return fine_traj_points_with_vel; + time_keeper_ptr_->toc(__func__, " "); } -std::vector ObstacleAvoidancePlanner::getExtendedTrajectory( - const std::vector & path_points, const std::vector & optimized_points) +std::vector ObstacleAvoidancePlanner::extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_traj_points) const { - stop_watch_.tic(__func__); - - assert(!path_points.empty()); + time_keeper_ptr_->tic(__func__); - const double accum_arc_length = motion_utils::calcArcLength(optimized_points); - if (accum_arc_length > traj_param_.trajectory_length) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), - "[Avoidance] Not extend trajectory"); - return std::vector{}; - } + const auto & joint_start_pose = optimized_traj_points.back().pose; // calculate end idx of optimized points on path points - const auto opt_end_path_idx = motion_utils::findNearestIndex( - path_points, optimized_points.back().pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - if (!opt_end_path_idx) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), - "[Avoidance] Not extend trajectory since could not find nearest idx from last opt point"); - return std::vector{}; - } - - auto extended_traj_points = [&]() -> std::vector { - const size_t non_fixed_begin_path_idx = opt_end_path_idx.get(); - const size_t non_fixed_end_path_idx = points_utils::findForwardIndex( - path_points, non_fixed_begin_path_idx, traj_param_.non_fixed_trajectory_length); - - if ( - non_fixed_begin_path_idx == path_points.size() - 1 || - non_fixed_begin_path_idx == non_fixed_end_path_idx) { - if (points_utils::isNearLastPathPoint( - optimized_points.back(), path_points, traj_param_.max_dist_for_extending_end_point, - traj_param_.delta_yaw_threshold_for_closest_point)) { - return std::vector{}; - } - const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); - return std::vector{last_traj_point}; - } else if (non_fixed_end_path_idx == path_points.size() - 1) { - // no need to connect smoothly since extended trajectory length is short enough - const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); - return std::vector{last_traj_point}; + const size_t joint_start_traj_seg_idx = + trajectory_utils::findEgoSegmentIndex(traj_points, joint_start_pose, ego_nearest_param_); + + // crop trajectory for extension + constexpr double joint_traj_length_for_smoothing = 5.0; + const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( + traj_points, joint_start_pose.position, joint_start_traj_seg_idx, + joint_traj_length_for_smoothing); + + // calculate full trajectory points + const auto full_traj_points = [&]() { + if (!joint_end_traj_point_idx) { + return optimized_traj_points; } - - // define non_fixed/fixed_traj_points - const auto begin_point = optimized_points.back(); - const auto end_point = - points_utils::convertToTrajectoryPoint(path_points.at(non_fixed_end_path_idx)); - const std::vector non_fixed_traj_points{begin_point, end_point}; - const std::vector fixed_path_points{ - path_points.begin() + non_fixed_end_path_idx + 1, path_points.end()}; - const auto fixed_traj_points = points_utils::convertToTrajectoryPoints(fixed_path_points); - - // spline interpolation to two traj points with end diff constraints - const double begin_yaw = tf2::getYaw(begin_point.pose.orientation); - const double end_yaw = tf2::getYaw(end_point.pose.orientation); - const auto interpolated_non_fixed_traj_points = - interpolation_utils::getConnectedInterpolatedPoints( - non_fixed_traj_points, eb_param_.delta_arc_length_for_eb, begin_yaw, end_yaw); - - // concat interpolated_non_fixed and fixed traj points - auto extended_points = interpolated_non_fixed_traj_points; - extended_points.insert( - extended_points.end(), fixed_traj_points.begin(), fixed_traj_points.end()); - - debug_data_.extended_non_fixed_traj = interpolated_non_fixed_traj_points; - debug_data_.extended_fixed_traj = fixed_traj_points; - - return extended_points; + const auto extended_traj_points = std::vector{ + traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; + return concatVectors(optimized_traj_points, extended_traj_points); }(); - // NOTE: Extended points will be concatenated with optimized points. - // Then, minimum velocity of each point is chosen from concatenated points or path points - // since optimized points has zero velocity outside drivable area - constexpr double large_velocity = 1e4; - for (auto & point : extended_traj_points) { - point.longitudinal_velocity_mps = large_velocity; - } - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return extended_traj_points; -} - -std::vector ObstacleAvoidancePlanner::generateFineTrajectoryPoints( - const std::vector & path_points, - const std::vector & traj_points) const -{ - stop_watch_.tic(__func__); - - // interpolate x and y - auto interpolated_traj_points = interpolation_utils::getInterpolatedTrajectoryPoints( - traj_points, traj_param_.delta_arc_length_for_trajectory); - - // calculate yaw from x and y - // NOTE: We do not use spline interpolation to yaw in behavior path since the yaw is unstable. - // Currently this implementation is removed since this calculation is heavy (~20ms) - // fillYawInTrajectoryPoint(interpolated_traj_points); - - // compensate last pose - points_utils::compensateLastPose( - path_points.back(), interpolated_traj_points, traj_param_.max_dist_for_extending_end_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - - return interpolated_traj_points; -} - -std::vector ObstacleAvoidancePlanner::alignVelocity( - const std::vector & fine_traj_points, const std::vector & path_points, - const std::vector & traj_points) const -{ - stop_watch_.tic(__func__); - - // insert zero velocity path index, and get optional zero_vel_path_idx - const auto path_zero_vel_info = - [&]() -> std::pair, boost::optional> { - const auto opt_path_zero_vel_idx = motion_utils::searchZeroVelocityIndex(path_points); - if (opt_path_zero_vel_idx) { - const auto & zero_vel_path_point = path_points.at(opt_path_zero_vel_idx.get()); - const auto opt_traj_seg_idx = motion_utils::findNearestSegmentIndex( - fine_traj_points, zero_vel_path_point.pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - if (opt_traj_seg_idx) { - const auto interpolated_pose = - lerpPose(fine_traj_points, zero_vel_path_point.pose.position, opt_traj_seg_idx.get()); - if (interpolated_pose) { - TrajectoryPoint zero_vel_traj_point; - zero_vel_traj_point.pose = interpolated_pose.get(); - zero_vel_traj_point.longitudinal_velocity_mps = - zero_vel_path_point.longitudinal_velocity_mps; - - if ( - tier4_autoware_utils::calcDistance2d( - fine_traj_points.at(opt_traj_seg_idx.get()).pose, zero_vel_traj_point.pose) < 1e-3) { - return {fine_traj_points, opt_traj_seg_idx.get()}; - } else if ( - tier4_autoware_utils::calcDistance2d( - fine_traj_points.at(opt_traj_seg_idx.get() + 1).pose, zero_vel_traj_point.pose) < - 1e-3) { - return {fine_traj_points, opt_traj_seg_idx.get() + 1}; - } - - auto fine_traj_points_with_zero_vel = fine_traj_points; - fine_traj_points_with_zero_vel.insert( - fine_traj_points_with_zero_vel.begin() + opt_traj_seg_idx.get() + 1, - zero_vel_traj_point); - return {fine_traj_points_with_zero_vel, opt_traj_seg_idx.get() + 1}; - } + // resample trajectory points + auto resampled_traj_points = trajectory_utils::resampleTrajectoryPoints( + full_traj_points, traj_param_.output_delta_arc_length); + + // update velocity on joint + for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + if (hasZeroVelocity(traj_points.at(i))) { + if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { + // Here is when current point is 0 velocity, but previous point is not 0 velocity. + const auto & input_stop_pose = traj_points.at(i).pose; + const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( + resampled_traj_points, input_stop_pose, ego_nearest_param_); + + // calculate and insert stop pose on output trajectory + trajectory_utils::insertStopPoint(resampled_traj_points, input_stop_pose, stop_seg_idx); } } - - return {fine_traj_points, {}}; - }(); - const auto fine_traj_points_with_path_zero_vel = path_zero_vel_info.first; - const auto opt_zero_vel_path_idx = path_zero_vel_info.second; - - // search zero velocity index of fine_traj_points - const size_t zero_vel_fine_traj_idx = [&]() { - // zero velocity for being outside drivable area - const size_t zero_vel_traj_idx = searchExtendedZeroVelocityIndex( - fine_traj_points_with_path_zero_vel, traj_points, - traj_param_.delta_yaw_threshold_for_closest_point); - - // zero velocity in path points - if (opt_zero_vel_path_idx) { - return std::min(opt_zero_vel_path_idx.get(), zero_vel_traj_idx); - } - return zero_vel_traj_idx; - }(); - - // interpolate z and velocity - auto fine_traj_points_with_vel = fine_traj_points_with_path_zero_vel; - size_t prev_begin_idx = 0; - for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { - auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); - if (truncated_points.size() < 2) { - // NOTE: At least, two points must be contained in truncated_points - truncated_points = std::vector( - path_points.begin() + prev_begin_idx, - path_points.begin() + std::min(path_points.size(), prev_begin_idx + 2)); - } - - const auto & target_pose = fine_traj_points_with_vel[i].pose; - const size_t closest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( - truncated_points, target_pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - // lerp z - fine_traj_points_with_vel[i].pose.position.z = - lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx); - - // lerp vx - const double target_vel = lerpTwistX(truncated_points, target_pose.position, closest_seg_idx); - - if (i >= zero_vel_fine_traj_idx) { - fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0; - } else { - fine_traj_points_with_vel[i].longitudinal_velocity_mps = target_vel; - } - - // NOTE: closest_seg_idx is for the clipped trajectory. This operation must be "+=". - prev_begin_idx += closest_seg_idx; } - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return fine_traj_points_with_vel; + // debug_data_ptr_->extended_traj_points = + // extended_traj_points ? *extended_traj_points : std::vector(); + time_keeper_ptr_->toc(__func__, " "); + return resampled_traj_points; } -void ObstacleAvoidancePlanner::publishDebugDataInMain(const Path & path) const +void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const { - stop_watch_.tic(__func__); - - { // publish trajectories - const auto debug_extended_fixed_traj = - createTrajectory(debug_data_.extended_fixed_traj, path.header); - debug_extended_fixed_traj_pub_->publish(debug_extended_fixed_traj); + time_keeper_ptr_->tic(__func__); - const auto debug_extended_non_fixed_traj = - createTrajectory(debug_data_.extended_non_fixed_traj, path.header); - debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); - } + // publish trajectories + const auto debug_extended_traj = + trajectory_utils::createTrajectory(header, debug_data_ptr_->extended_traj_points); + debug_extended_traj_pub_->publish(debug_extended_traj); - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + time_keeper_ptr_->toc(__func__, " "); } +} // namespace obstacle_avoidance_planner #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(ObstacleAvoidancePlanner) +RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_avoidance_planner::ObstacleAvoidancePlanner) diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp new file mode 100644 index 0000000000000..42ebab7f14e5d --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -0,0 +1,158 @@ +// Copyright 2023 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 "obstacle_avoidance_planner/replan_checker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace obstacle_avoidance_planner +{ +ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) +: ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) +{ + max_path_shape_around_ego_lat_dist_ = + node->declare_parameter("replan.max_path_shape_around_ego_lat_dist"); + max_ego_moving_dist_ = node->declare_parameter("replan.max_ego_moving_dist"); + max_goal_moving_dist_ = node->declare_parameter("replan.max_goal_moving_dist"); + max_delta_time_sec_ = node->declare_parameter("replan.max_delta_time_sec"); +} + +void ReplanChecker::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam( + parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); + updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); +} + +bool ReplanChecker::isResetRequired(const PlannerData & planner_data) +{ + const auto & p = planner_data; + + const bool reset_required = [&]() { + // guard for invalid variables + if (!prev_traj_points_ptr_ || !prev_ego_pose_ptr_) { + return true; + } + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // path shape changes + if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path shape changed."); + return true; + } + + // path goal changes + if (isPathGoalChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + return true; + } + + // ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + if (max_ego_moving_dist_ < delta_dist) { + RCLCPP_INFO( + logger_, + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + return true; + } + + return false; + }(); + + // update previous information required in this function + prev_traj_points_ptr_ = std::make_shared>(p.traj_points); + prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); + + return reset_required; +} + +bool ReplanChecker::isReplanRequired(const rclcpp::Time & current_time) +{ + const bool replan_required = [&]() { + // guard for invalid variables + if (!prev_replanned_time_ptr_ || !prev_traj_points_ptr_) { + return true; + } + + // time elapses + const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); + if (max_delta_time_sec_ < delta_time_sec) { + return true; + } + + return false; + }(); + + // update previous information required in this function + if (replan_required) { + prev_replanned_time_ptr_ = std::make_shared(current_time); + } + + return replan_required; +} + +bool ReplanChecker::isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate ego's lateral offset to previous trajectory points + const auto prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + const double prev_ego_lat_offset = + motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + + // calculate ego's lateral offset to current trajectory points + const auto ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const double ego_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + + const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; + if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { + return false; + } + + return true; +} + +bool ReplanChecker::isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // check if the vehicle is stopping + constexpr double min_vel = 1e-3; + if (min_vel < std::abs(p.ego_vel)) { + return false; + } + + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + if (goal_moving_dist < max_goal_moving_dist_) { + return false; + } + + return true; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp new file mode 100644 index 0000000000000..a6eaf7ffdd2ac --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 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 "obstacle_avoidance_planner/state_equation_generator.hpp" + +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" + +namespace obstacle_avoidance_planner +{ +// state equation: x = B u + W (u includes x_0) +// NOTE: Originally, x_t+1 = Ad x_t + Bd u + Wd. +StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( + const std::vector & ref_points) const +{ + time_keeper_ptr_->tic(__func__); + + const size_t N_ref = ref_points.size(); + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t D_v = D_x + D_u * (N_ref - 1); + + // matrices for whole state equation + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); + Eigen::VectorXd W = Eigen::VectorXd::Zero(D_x * N_ref); + + // matrices for one-step state equation + Eigen::MatrixXd Ad(D_x, D_x); + Eigen::MatrixXd Bd(D_x, D_u); + Eigen::MatrixXd Wd(D_x, 1); + + // calculate one-step state equation considering kinematics N_ref times + for (size_t i = 0; i < N_ref; ++i) { + if (i == 0) { + B.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); + continue; + } + + const int idx_x_i = i * D_x; + const int idx_x_i_prev = (i - 1) * D_x; + const int idx_u_i_prev = (i - 1) * D_u; + + // get discrete kinematics matrix A, B, W + const auto & p = ref_points.at(i - 1); + + // TODO(murooka) use curvature by stabling optimization + // Currently, when using curvature, the optimization result is weird with sample_map. + // vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, p.curvature, + // p.delta_arc_length); + vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, 0.0, p.delta_arc_length); + + B.block(idx_x_i, 0, D_x, D_x) = Ad * B.block(idx_x_i_prev, 0, D_x, D_x); + B.block(idx_x_i, D_x + idx_u_i_prev, D_x, D_u) = Bd; + + for (size_t j = 0; j < i - 1; ++j) { + size_t idx_u_j = j * D_u; + B.block(idx_x_i, D_x + idx_u_j, D_x, D_u) = + Ad * B.block(idx_x_i_prev, D_x + idx_u_j, D_x, D_u); + } + + W.segment(idx_x_i, D_x) = Ad * W.block(idx_x_i_prev, 0, D_x, 1) + Wd; + } + + Matrix mat; + mat.B = B; + mat.W = W; + + time_keeper_ptr_->toc(__func__, " "); + return mat; +} + +Eigen::VectorXd StateEquationGenerator::predict( + const StateEquationGenerator::Matrix & mat, const Eigen::VectorXd U) const +{ + return mat.B * U + mat.W; +} +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp deleted file mode 100644 index eb689301dd88a..0000000000000 --- a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp +++ /dev/null @@ -1,858 +0,0 @@ -// Copyright 2020 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 "obstacle_avoidance_planner/utils/debug_utils.hpp" - -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "tf2/utils.h" - -#include "visualization_msgs/msg/marker.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include -#include - -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; - -namespace -{ -template -visualization_msgs::msg::MarkerArray getPointsMarkerArray( - const std::vector & points, const std::string & ns, const double r, const double g, - const double b) -{ - if (points.empty()) { - return visualization_msgs::msg::MarkerArray{}; - } - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - - for (const auto & point : points) { - marker.points.push_back(tier4_autoware_utils::getPoint(point)); - } - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} - -template -visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( - const std::vector & points, const std::string & ns, const double r, const double g, - const double b) -{ - if (points.empty()) { - return visualization_msgs::msg::MarkerArray{}; - } - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 0.15), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - visualization_msgs::msg::MarkerArray msg; - for (size_t i = 0; i < points.size(); i++) { - marker.id = i; - marker.text = std::to_string(i); - marker.pose.position = tier4_autoware_utils::getPoint(points[i]); - msg.markers.push_back(marker); - } - - return msg; -} - -geometry_msgs::msg::Pose getVirtualWallPose( - const geometry_msgs::msg::Pose & target_pose, const VehicleParam & vehicle_param) -{ - const double base_link2front = vehicle_param.wheelbase + vehicle_param.front_overhang; - tf2::Transform tf_base_link2front( - tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(base_link2front, 0.0, 0.0)); - tf2::Transform tf_map2base_link; - tf2::fromMsg(target_pose, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - geometry_msgs::msg::Pose virtual_wall_pose; - tf2::toMsg(tf_map2front, virtual_wall_pose); - return virtual_wall_pose; -} - -visualization_msgs::msg::MarkerArray getDebugPointsMarkers( - const std::vector & interpolated_points, - const std::vector & optimized_points, - const std::vector & straight_points, - const std::vector & fixed_points, - const std::vector & non_fixed_points) -{ - visualization_msgs::msg::MarkerArray marker_array; - int unique_id = 0; - - unique_id = 0; - visualization_msgs::msg::Marker interpolated_points_marker; - interpolated_points_marker.lifetime = rclcpp::Duration::from_seconds(0); - interpolated_points_marker.header.frame_id = "map"; - interpolated_points_marker.header.stamp = rclcpp::Time(0); - interpolated_points_marker.ns = std::string("interpolated_points_marker"); - interpolated_points_marker.action = visualization_msgs::msg::Marker::ADD; - interpolated_points_marker.pose.orientation.w = 1.0; - interpolated_points_marker.id = unique_id; - interpolated_points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - interpolated_points_marker.scale = createMarkerScale(.3, .3, .3); - interpolated_points_marker.color = createMarkerColor(1.0f, 0, 0, 0.8); - unique_id++; - for (const auto & point : interpolated_points) { - interpolated_points_marker.points.push_back(point); - } - if (!interpolated_points_marker.points.empty()) { - marker_array.markers.push_back(interpolated_points_marker); - } - - unique_id = 0; - visualization_msgs::msg::Marker optimized_points_marker; - optimized_points_marker.lifetime = rclcpp::Duration::from_seconds(0); - optimized_points_marker.header.frame_id = "map"; - optimized_points_marker.header.stamp = rclcpp::Time(0); - optimized_points_marker.ns = std::string("optimized_points_marker"); - optimized_points_marker.action = visualization_msgs::msg::Marker::ADD; - optimized_points_marker.pose.orientation.w = 1.0; - optimized_points_marker.id = unique_id; - optimized_points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - optimized_points_marker.scale = createMarkerScale(0.35, 0.35, 0.35); - optimized_points_marker.color = createMarkerColor(0, 1.0f, 0, 0.99); - unique_id++; - for (const auto & point : optimized_points) { - optimized_points_marker.points.push_back(point.pose.position); - } - if (!optimized_points_marker.points.empty()) { - marker_array.markers.push_back(optimized_points_marker); - } - - unique_id = 0; - for (size_t i = 0; i < optimized_points.size(); i++) { - visualization_msgs::msg::Marker optimized_points_text_marker; - optimized_points_text_marker.lifetime = rclcpp::Duration::from_seconds(0); - optimized_points_text_marker.header.frame_id = "map"; - optimized_points_text_marker.header.stamp = rclcpp::Time(0); - optimized_points_text_marker.ns = std::string("optimized_points_text_marker"); - optimized_points_text_marker.action = visualization_msgs::msg::Marker::ADD; - optimized_points_text_marker.pose.orientation.w = 1.0; - optimized_points_text_marker.id = unique_id; - optimized_points_text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - optimized_points_text_marker.pose.position = optimized_points[i].pose.position; - optimized_points_text_marker.scale = createMarkerScale(0, 0, 0.15); - optimized_points_text_marker.color = createMarkerColor(0, 1.0, 0, 0.99); - optimized_points_text_marker.text = std::to_string(i); - unique_id++; - marker_array.markers.push_back(optimized_points_text_marker); - } - - unique_id = 0; - for (size_t i = 0; i < interpolated_points.size(); i++) { - visualization_msgs::msg::Marker interpolated_points_text_marker; - interpolated_points_text_marker.lifetime = rclcpp::Duration::from_seconds(0); - interpolated_points_text_marker.header.frame_id = "map"; - interpolated_points_text_marker.header.stamp = rclcpp::Time(0); - interpolated_points_text_marker.ns = std::string("interpolated_points_text_marker"); - interpolated_points_text_marker.action = visualization_msgs::msg::Marker::ADD; - interpolated_points_text_marker.pose.orientation.w = 1.0; - interpolated_points_text_marker.id = unique_id; - interpolated_points_text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - interpolated_points_text_marker.pose.position = interpolated_points[i]; - interpolated_points_text_marker.scale = createMarkerScale(0, 0, 0.5); - interpolated_points_text_marker.color = createMarkerColor(0, 1.0, 0, 0.99); - interpolated_points_text_marker.text = std::to_string(i); - unique_id++; - marker_array.markers.push_back(interpolated_points_text_marker); - } - - unique_id = 0; - visualization_msgs::msg::Marker straight_points_marker; - straight_points_marker.lifetime = rclcpp::Duration::from_seconds(40); - straight_points_marker.header.frame_id = "map"; - straight_points_marker.header.stamp = rclcpp::Time(0); - straight_points_marker.ns = std::string("straight_points_marker"); - straight_points_marker.action = visualization_msgs::msg::Marker::ADD; - straight_points_marker.pose.orientation.w = 1.0; - straight_points_marker.id = unique_id; - straight_points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - straight_points_marker.scale = createMarkerScale(1.0, 1.0, 1.0); - straight_points_marker.color = createMarkerColor(1.0, 1.0, 0, 0.99); - unique_id++; - for (const auto & point : straight_points) { - straight_points_marker.points.push_back(point); - } - if (!straight_points_marker.points.empty()) { - marker_array.markers.push_back(straight_points_marker); - } - - unique_id = 0; - visualization_msgs::msg::Marker fixed_marker; - fixed_marker.lifetime = rclcpp::Duration::from_seconds(0); - fixed_marker.header.frame_id = "map"; - fixed_marker.header.stamp = rclcpp::Time(0); - fixed_marker.ns = std::string("fixed_points_marker"); - fixed_marker.action = visualization_msgs::msg::Marker::ADD; - fixed_marker.pose.orientation.w = 1.0; - fixed_marker.id = unique_id; - fixed_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - fixed_marker.scale = createMarkerScale(0.3, 0.3, 0.3); - fixed_marker.color = createMarkerColor(1.0, 0, 0, 0.99); - unique_id++; - for (const auto & point : fixed_points) { - fixed_marker.points.push_back(point.position); - } - if (!fixed_marker.points.empty()) { - marker_array.markers.push_back(fixed_marker); - } - - visualization_msgs::msg::Marker non_fixed_marker; - non_fixed_marker.lifetime = rclcpp::Duration::from_seconds(20); - non_fixed_marker.header.frame_id = "map"; - non_fixed_marker.header.stamp = rclcpp::Time(0); - non_fixed_marker.ns = std::string("non_fixed_points_marker"); - non_fixed_marker.action = visualization_msgs::msg::Marker::ADD; - non_fixed_marker.pose.orientation.w = 1.0; - non_fixed_marker.id = unique_id; - non_fixed_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - non_fixed_marker.scale = createMarkerScale(1.0, 1.0, 1.0); - non_fixed_marker.color = createMarkerColor(0, 1.0, 0, 0.99); - unique_id++; - for (const auto & point : non_fixed_points) { - non_fixed_marker.points.push_back(point.position); - } - if (!non_fixed_marker.points.empty()) { - marker_array.markers.push_back(non_fixed_marker); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray getDebugConstrainMarkers( - const std::vector & constrain_ranges, const std::string & ns) -{ - visualization_msgs::msg::MarkerArray marker_array; - int unique_id = 0; - for (size_t i = 0; i < constrain_ranges.size(); i++) { - visualization_msgs::msg::Marker constrain_rect_marker; - constrain_rect_marker.lifetime = rclcpp::Duration::from_seconds(0); - constrain_rect_marker.header.frame_id = "map"; - constrain_rect_marker.header.stamp = rclcpp::Time(0); - constrain_rect_marker.ns = ns; - constrain_rect_marker.action = visualization_msgs::msg::Marker::ADD; - constrain_rect_marker.pose.orientation.w = 1.0; - constrain_rect_marker.id = unique_id; - constrain_rect_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - constrain_rect_marker.scale = createMarkerScale(0.01, 0, 0); - constrain_rect_marker.color = createMarkerColor(1.0, 0, 0, 0.99); - unique_id++; - geometry_msgs::msg::Point top_left_point = constrain_ranges[i].top_left; - geometry_msgs::msg::Point top_right_point = constrain_ranges[i].top_right; - geometry_msgs::msg::Point bottom_right_point = constrain_ranges[i].bottom_right; - geometry_msgs::msg::Point bottom_left_point = constrain_ranges[i].bottom_left; - constrain_rect_marker.points.push_back(top_left_point); - constrain_rect_marker.points.push_back(top_right_point); - constrain_rect_marker.points.push_back(bottom_right_point); - constrain_rect_marker.points.push_back(bottom_left_point); - constrain_rect_marker.points.push_back(top_left_point); - marker_array.markers.push_back(constrain_rect_marker); - } - - for (size_t i = 0; i < constrain_ranges.size(); i++) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(0); - marker.ns = ns + "_text"; - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.scale = createMarkerScale(0, 0, 0.15); - marker.color = createMarkerColor(1.0, 0, 0, 0.99); - marker.text = std::to_string(i); - marker.pose.position = constrain_ranges[i].top_left; - marker_array.markers.push_back(marker); - } - - unique_id = 0; - for (size_t i = 0; i < constrain_ranges.size(); i++) { - visualization_msgs::msg::Marker constrain_range_text_marker; - constrain_range_text_marker.lifetime = rclcpp::Duration::from_seconds(0); - constrain_range_text_marker.header.frame_id = "map"; - constrain_range_text_marker.header.stamp = rclcpp::Time(0); - constrain_range_text_marker.ns = ns + "_location"; - constrain_range_text_marker.action = visualization_msgs::msg::Marker::ADD; - constrain_range_text_marker.pose.orientation.w = 1.0; - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - constrain_range_text_marker.pose.position = constrain_ranges[i].top_left; - constrain_range_text_marker.scale = createMarkerScale(0, 0, 0.1); - constrain_range_text_marker.color = createMarkerColor(1.0, 0, 0, 0.99); - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.pose.position = constrain_ranges[i].top_right; - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.pose.position = constrain_ranges[i].bottom_left; - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - - constrain_range_text_marker.id = unique_id; - constrain_range_text_marker.pose.position = constrain_ranges[i].bottom_right; - constrain_range_text_marker.text = std::to_string(i) + std::string(" x ") + - std::to_string(constrain_range_text_marker.pose.position.x) + - std::string("y ") + - std::to_string(constrain_range_text_marker.pose.position.y); - unique_id++; - marker_array.markers.push_back(constrain_range_text_marker); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray getObjectsMarkerArray( - const std::vector & objects, - const std::string & ns, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(3.0, 1.0, 1.0), createMarkerColor(r, g, b, 0.8)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < objects.size(); ++i) { - const auto & object = objects.at(i); - marker.id = i; - marker.pose = object.kinematics.initial_pose_with_covariance.pose; - msg.markers.push_back(marker); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getRectanglesMarkerArray( - const std::vector mpt_traj, - const VehicleParam & vehicle_param, const std::string & ns, const double r, const double g, - const double b, const size_t sampling_num) -{ - visualization_msgs::msg::MarkerArray msg; - for (size_t i = 0; i < mpt_traj.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - const auto & traj_point = mpt_traj.at(i); - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) - .position); - marker.points.push_back(marker.points.front()); - - msg.markers.push_back(marker); - } - return msg; -} - -visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray( - const std::vector mpt_traj, - const VehicleParam & vehicle_param, const std::string & ns, const double r, const double g, - const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 0.125), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - visualization_msgs::msg::MarkerArray msg; - for (size_t i = 0; i < mpt_traj.size(); ++i) { - const auto & traj_point = mpt_traj.at(i); - - marker.text = std::to_string(i); - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - - const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) - .position; - marker.id = i; - marker.pose.position = top_right_pos; - msg.markers.push_back(marker); - - marker.id = i + mpt_traj.size(); - marker.pose.position = top_right_pos; - msg.markers.push_back(marker); - } - return msg; -} - -visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( - const std::vector & ref_points, std::vector & bounds_candidates, - const double r, const double g, const double b, [[maybe_unused]] const double vehicle_width, - const size_t sampling_num) -{ - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - const std::string ns = "bounds_candidates"; - - if (ref_points.empty() || bounds_candidates.empty()) return msg; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < ref_points.size(); i++) { - if (i % sampling_num != 0) { - continue; - } - - const auto & bound_candidates = bounds_candidates.at(i); - geometry_msgs::msg::Pose pose; - pose.position = ref_points.at(i).p; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); - - // lower bound - const double lb_y = bound_candidates.lower_bound; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; - marker.points.push_back(lb); - - // upper bound - const double ub_y = bound_candidates.upper_bound; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; - marker.points.push_back(ub); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray( - const std::vector & ref_points, const VehicleParam & vehicle_param, - const double r, const double g, const double b, const size_t sampling_num) -{ - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - - if (ref_points.empty()) return msg; - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - - for (size_t bound_idx = 0; bound_idx < ref_points.at(0).vehicle_bounds.size(); ++bound_idx) { - const std::string ns = "base_bounds_" + std::to_string(bound_idx); - - { // lower bound - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < ref_points.size(); i++) { - if (i % sampling_num != 0) { - continue; - } - - const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx); - const double lb_y = ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - base_to_right; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; - - marker.points.push_back(pose.position); - marker.points.push_back(lb); - } - msg.markers.push_back(marker); - } - - { // upper bound - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g + 0.5, b, 0.3)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < ref_points.size(); i++) { - if (i % sampling_num != 0) { - continue; - } - - const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx); - const double ub_y = ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + base_to_left; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; - - marker.points.push_back(pose.position); - marker.points.push_back(ub); - } - msg.markers.push_back(marker); - } - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray( - const std::vector> & vehicle_circles_pose, - const VehicleParam & vehicle_param, const size_t sampling_num, const std::string & ns, - const double r, const double g, const double b) -{ - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - for (size_t i = 0; i < vehicle_circles_pose.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 0.25)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t j = 0; j < vehicle_circles_pose.at(i).size(); ++j) { - const geometry_msgs::msg::Pose & pose = vehicle_circles_pose.at(i).at(j); - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, base_to_left, 0.0).position; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, -base_to_right, 0.0).position; - - marker.points.push_back(ub); - marker.points.push_back(lb); - } - msg.markers.push_back(marker); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray getLateralErrorsLineMarkerArray( - const std::vector mpt_ref_poses, std::vector lateral_errors, - const size_t sampling_num, const std::string & ns, const double r, const double g, const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 1.0)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - for (size_t i = 0; i < mpt_ref_poses.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - - const auto vehicle_pose = - tier4_autoware_utils::calcOffsetPose(mpt_ref_poses.at(i), 0.0, lateral_errors.at(i), 0.0); - marker.points.push_back(mpt_ref_poses.at(i).position); - marker.points.push_back(vehicle_pose.position); - } - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} - -visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray( - const geometry_msgs::msg::Pose & current_ego_pose, const VehicleParam & vehicle_param, - const std::vector & vehicle_circle_longitudinal_offsets, - const std::vector & vehicle_circle_radiuses, const std::string & ns, const double r, - const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - size_t id = 0; - for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { - const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(current_ego_pose, offset, 0.0, 0.0); - - constexpr size_t circle_dividing_num = 16; - for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { - geometry_msgs::msg::Point edge_pos; - - const double edge_angle = - static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; - edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle) + lateral_offset; - edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle) - lateral_offset; - - marker.points.push_back(edge_pos); - } - - msg.markers.push_back(marker); - ++id; - } - return msg; -} - -visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray( - const std::vector & mpt_traj_points, - [[maybe_unused]] const VehicleParam & vehicle_param, - const std::vector & vehicle_circle_longitudinal_offsets, - const std::vector & vehicle_circle_radiuses, const size_t sampling_num, - const std::string & ns, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - size_t id = 0; - for (size_t i = 0; i < mpt_traj_points.size(); ++i) { - if (i % sampling_num != 0) { - continue; - } - const auto & mpt_traj_point = mpt_traj_points.at(i); - - for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { - const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); - const double lateral_offset = - abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); - - constexpr size_t circle_dividing_num = 16; - for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { - geometry_msgs::msg::Point edge_pos; - - const double edge_angle = - static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; - edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle) + lateral_offset; - edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle) - lateral_offset; - - marker.points.push_back(edge_pos); - } - - msg.markers.push_back(marker); - ++id; - } - } - return msg; -} - -visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( - const geometry_msgs::msg::Pose & stop_pose, const VehicleParam & vehicle_param, - const std::string & ns, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0).position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0).position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); - marker.points.push_back(marker.points.front()); - - msg.markers.push_back(marker); - - return msg; -} - -visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray( - const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, - const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(r, g, b, 0.5)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = pose; - marker.pose.position.z += 1.0; - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} - -visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray( - const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, - const double b) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(r, g, b, 0.99)); - marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = pose; - marker.pose.position.z += 2.0; - marker.text = "drivable area"; - - visualization_msgs::msg::MarkerArray msg; - msg.markers.push_back(marker); - - return msg; -} -} // namespace - -namespace debug_utils -{ -visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - DebugData & debug_data, - const std::vector & optimized_points, - const VehicleParam & vehicle_param, const bool is_showing_debug_detail) -{ - visualization_msgs::msg::MarkerArray vis_marker_array; - - if (is_showing_debug_detail) { - const auto points_marker_array = getDebugPointsMarkers( - debug_data.interpolated_points, optimized_points, debug_data.straight_points, - debug_data.fixed_points, debug_data.non_fixed_points); - - const auto constrain_marker_array = - getDebugConstrainMarkers(debug_data.constrain_rectangles, "constrain_rect"); - - appendMarkerArray(points_marker_array, &vis_marker_array); - appendMarkerArray(constrain_marker_array, &vis_marker_array); - - appendMarkerArray( - getRectanglesNumMarkerArray( - debug_data.mpt_traj, vehicle_param, "num_vehicle_footprint", 0.99, 0.99, 0.2), - &vis_marker_array); - - appendMarkerArray( - getPointsTextMarkerArray(debug_data.eb_traj, "eb_traj_text", 0.99, 0.99, 0.2), - &vis_marker_array); - } - - // avoiding objects - appendMarkerArray( - getObjectsMarkerArray(debug_data.avoiding_objects, "avoiding_objects", 0.99, 0.99, 0.2), - &vis_marker_array); - // mpt footprints - appendMarkerArray( - getRectanglesMarkerArray( - debug_data.mpt_traj, vehicle_param, "mpt_footprints", 0.99, 0.99, 0.2, - debug_data.mpt_visualize_sampling_num), - &vis_marker_array); - // bounds - appendMarkerArray( - getBoundsLineMarkerArray( - debug_data.ref_points, vehicle_param, 0.99, 0.99, 0.2, debug_data.mpt_visualize_sampling_num), - &vis_marker_array); - - // bounds candidates - appendMarkerArray( - getBoundsCandidatesLineMarkerArray( - debug_data.ref_points, debug_data.bounds_candidates, 0.2, 0.99, 0.99, vehicle_param.width, - debug_data.mpt_visualize_sampling_num), - &vis_marker_array); - - // vehicle circle line - appendMarkerArray( - getVehicleCircleLineMarkerArray( - debug_data.vehicle_circles_pose, vehicle_param, debug_data.mpt_visualize_sampling_num, - "vehicle_circle_lines", 0.99, 0.99, 0.2), - &vis_marker_array); - - // lateral error line - appendMarkerArray( - getLateralErrorsLineMarkerArray( - debug_data.mpt_ref_poses, debug_data.lateral_errors, debug_data.mpt_visualize_sampling_num, - "lateral_errors", 0.1, 0.1, 0.8), - &vis_marker_array); - - // current vehicle circles - appendMarkerArray( - getCurrentVehicleCirclesMarkerArray( - debug_data.current_ego_pose, vehicle_param, debug_data.vehicle_circle_longitudinal_offsets, - debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), - &vis_marker_array); - - // vehicle circles - appendMarkerArray( - getVehicleCirclesMarkerArray( - debug_data.mpt_traj, vehicle_param, debug_data.vehicle_circle_longitudinal_offsets, - debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles", - 1.0, 0.3, 0.3), - &vis_marker_array); - - // footprint by drivable area - if (debug_data.stop_pose_by_drivable_area) { - appendMarkerArray( - getFootprintByDrivableAreaMarkerArray( - *debug_data.stop_pose_by_drivable_area, vehicle_param, "footprint_by_drivable_area", 1.0, - 0.0, 0.0), - &vis_marker_array); - } - - return vis_marker_array; -} - -visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( - DebugData & debug_data, const VehicleParam & vehicle_param) -{ - visualization_msgs::msg::MarkerArray vis_marker_array; - if (debug_data.stop_pose_by_drivable_area) { - const auto virtual_wall_pose = - getVirtualWallPose(debug_data.stop_pose_by_drivable_area.get(), vehicle_param); - appendMarkerArray( - getVirtualWallMarkerArray(virtual_wall_pose, "virtual_wall", 1.0, 0, 0), &vis_marker_array); - appendMarkerArray( - getVirtualWallTextMarkerArray(virtual_wall_pose, "virtual_wall_text", 1.0, 1.0, 1.0), - &vis_marker_array); - } - return vis_marker_array; -} -} // namespace debug_utils diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp new file mode 100644 index 0000000000000..e471baf1951db --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp @@ -0,0 +1,205 @@ +// Copyright 2023 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 "obstacle_avoidance_planner/utils/geometry_utils.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include +#include +#include + +namespace obstacle_avoidance_planner +{ +namespace bg = boost::geometry; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +namespace +{ +geometry_msgs::msg::Point getStartPoint( + const std::vector & bound, const geometry_msgs::msg::Point & point) +{ + const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); + const auto & curr_seg_point = bound.at(segment_idx); + const auto & next_seg_point = bound.at(segment_idx); + const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; + const Eigen::Vector2d first_to_second{ + next_seg_point.x - curr_seg_point.x, next_seg_point.y - curr_seg_point.y}; + const double length = first_to_target.dot(first_to_second.normalized()); + + if (length < 0.0) { + return bound.front(); + } + + const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); + if (first_point) { + return *first_point; + } + + return bound.front(); +} + +bool isFrontDrivableArea( + const geometry_msgs::msg::Point & point, + const std::vector & left_bound, + const std::vector & right_bound) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + constexpr double min_dist = 0.1; + const auto left_start_point = getStartPoint(left_bound, right_bound.front()); + const auto right_start_point = getStartPoint(right_bound, left_bound.front()); + + // ignore point behind of the front line + const std::vector front_bound = {left_start_point, right_start_point}; + const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); + if (lat_dist_to_front_bound < min_dist) { + return true; + } + + return false; +} + +Polygon2d createDrivablePolygon( + const std::vector & left_bound, + const std::vector & right_bound) +{ + Polygon2d drivable_area_poly; + + // left bound + for (const auto & p : left_bound) { + drivable_area_poly.outer().push_back(Point2d(p.x, p.y)); + } + + // right bound + auto reversed_right_bound = right_bound; + std::reverse(reversed_right_bound.begin(), reversed_right_bound.end()); + for (const auto & p : reversed_right_bound) { + drivable_area_poly.outer().push_back(Point2d(p.x, p.y)); + } + + drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); + return drivable_area_poly; +} +} // namespace + +namespace geometry_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, + const std::vector & right_bound, + const vehicle_info_util::VehicleInfo & vehicle_info, + const bool use_footprint_polygon_for_outside_drivable_area_check) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + const double base_to_right = vehicle_info.wheel_tread_m / 2.0 + vehicle_info.right_overhang_m; + const double base_to_left = vehicle_info.wheel_tread_m / 2.0 + vehicle_info.left_overhang_m; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + // calculate footprint corner points + const auto top_left_pos = + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; + const auto top_right_pos = + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; + const auto bottom_right_pos = + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; + const auto bottom_left_pos = + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; + + if (use_footprint_polygon_for_outside_drivable_area_check) { + // calculate footprint polygon + LinearRing2d footprint_polygon; + footprint_polygon.push_back({top_left_pos.x, top_left_pos.y}); + footprint_polygon.push_back({top_right_pos.x, top_right_pos.y}); + footprint_polygon.push_back({bottom_right_pos.x, bottom_right_pos.y}); + footprint_polygon.push_back({bottom_left_pos.x, bottom_left_pos.y}); + bg::correct(footprint_polygon); + + // calculate boundary line strings + LineString2d left_bound_line; + for (const auto & p : left_bound) { + left_bound_line.push_back({p.x, p.y}); + } + + LineString2d right_bound_line; + for (const auto & p : right_bound) { + right_bound_line.push_back({p.x, p.y}); + } + + LineString2d back_bound_line; + back_bound_line = {left_bound_line.back(), right_bound_line.back()}; + + if ( + bg::intersects(footprint_polygon, left_bound_line) || + bg::intersects(footprint_polygon, right_bound_line) || + bg::intersects(footprint_polygon, back_bound_line)) { + return true; + } + return false; + } + + const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound); + const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound); + const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound); + const bool front_bottom_right = isFrontDrivableArea(bottom_right_pos, left_bound, right_bound); + + // create rectangle + const auto drivable_area_poly = createDrivablePolygon(left_bound, right_bound); + + if (!front_top_left && !bg::within(Point2d(top_left_pos.x, top_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_top_right && + !bg::within(Point2d(top_right_pos.x, top_right_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_left && + !bg::within(Point2d(bottom_left_pos.x, bottom_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_right && + !bg::within(Point2d(bottom_right_pos.x, bottom_right_pos.y), drivable_area_poly)) { + return true; + } + + return false; +} +} // namespace geometry_utils +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp new file mode 100644 index 0000000000000..8a4ab4384f7eb --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -0,0 +1,235 @@ +// Copyright 2023 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 "obstacle_avoidance_planner/utils/trajectory_utils.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/eb_path_smoother.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p) +{ + return p.pose.position; +} + +template <> +geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p) +{ + return p.pose; +} + +template <> +double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p) +{ + return p.longitudinal_velocity_mps; +} +} // namespace tier4_autoware_utils + +namespace obstacle_avoidance_planner +{ +namespace trajectory_utils +{ +ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point) +{ + ReferencePoint ref_point; + + ref_point.pose = traj_point.pose; + ref_point.longitudinal_velocity_mps = traj_point.longitudinal_velocity_mps; + return ref_point; +} + +std::vector convertToReferencePoints( + const std::vector & traj_points) +{ + std::vector ref_points; + for (const auto & traj_point : traj_points) { + const auto ref_point = convertToReferencePoint(traj_point); + ref_points.push_back(ref_point); + } + + return ref_points; +} + +void compensateLastPose( + const PathPoint & last_path_point, std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold) +{ + if (traj_points.empty()) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + return; + } + + const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; + + const double dist = + tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double norm_diff_yaw = [&]() { + const double diff_yaw = + tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); + return tier4_autoware_utils::normalizeRadian(diff_yaw); + }(); + if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + } +} + +geometry_msgs::msg::Point getNearestPosition( + const std::vector & points, const int target_idx, const double offset) +{ + double sum_arc_length = 0.0; + for (size_t i = target_idx; i < points.size(); ++i) { + sum_arc_length += points.at(i).delta_arc_length; + + if (offset < sum_arc_length) { + return points.at(i).pose.position; + } + } + + return points.back().pose.position; +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & traj_points) +{ + auto traj = motion_utils::convertToTrajectory(traj_points); + traj.header = header; + + return traj; +} + +std::vector resampleTrajectoryPoints( + const std::vector traj_points, const double interval) +{ + constexpr bool enable_resampling_stop_point = true; + + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory( + traj, interval, false, true, true, enable_resampling_stop_point); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +// NOTE: stop point will not be resampled +std::vector resampleTrajectoryPointsWithoutStopPoint( + const std::vector traj_points, const double interval) +{ + constexpr bool enable_resampling_stop_point = false; + + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory( + traj, interval, false, true, true, enable_resampling_stop_point); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +std::vector resampleReferencePoints( + const std::vector ref_points, const double interval) +{ + // resample pose and velocity + const auto traj_points = convertToTrajectoryPoints(ref_points); + const auto resampled_traj_points = + resampleTrajectoryPointsWithoutStopPoint(traj_points, interval); + const auto resampled_ref_points = convertToReferencePoints(resampled_traj_points); + + // resample curvature + std::vector base_keys; + std::vector base_values; + for (size_t i = 0; i < ref_points.size(); ++i) { + if (i == 0) { + base_keys.push_back(0.0); + } else { + const double delta_arc_length = + tier4_autoware_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); + base_keys.push_back(base_keys.back() + delta_arc_length); + } + + base_values.push_back(ref_points.at(i).curvature); + } + + std::vector query_keys; + for (size_t i = 0; i < resampled_ref_points.size(); ++i) { + if (i == 0) { + query_keys.push_back(0.0); + } else { + const double delta_arc_length = tier4_autoware_utils::calcDistance2d( + resampled_ref_points.at(i), resampled_ref_points.at(i - 1)); + const double key = query_keys.back() + delta_arc_length; + if (base_keys.back() < key) { + break; + } + + query_keys.push_back(key); + } + } + + if (query_keys.size() != resampled_ref_points.size()) { + // compensate last key + constexpr double epsilon = 1e-6; + query_keys.push_back(base_keys.back() - epsilon); + } + + const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + + // create output reference points by updating curvature with resampled one + std::vector output_ref_points; + for (size_t i = 0; i < query_values.size(); ++i) { + output_ref_points.push_back(resampled_ref_points.at(i)); + output_ref_points.at(i).curvature = query_values.at(i); + } + + return output_ref_points; +} + +void insertStopPoint( + std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, + const size_t stop_seg_idx) +{ + const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + traj_points, stop_seg_idx, input_stop_pose.position); + + const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); + + if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { + traj_points.at(stop_seg_idx).longitudinal_velocity_mps = 0.0; + return; + } + if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx + 1), stop_pose)) { + traj_points.at(stop_seg_idx + 1).longitudinal_velocity_mps = 0.0; + return; + } + + TrajectoryPoint additional_traj_point; + additional_traj_point.pose = stop_pose; + additional_traj_point.longitudinal_velocity_mps = 0.0; + + traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); +} +} // namespace trajectory_utils +} // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp deleted file mode 100644 index 377cb8150d60c..0000000000000 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ /dev/null @@ -1,780 +0,0 @@ -// Copyright 2020 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 "obstacle_avoidance_planner/utils/utils.hpp" - -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "tf2/utils.h" - -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include -#include -#include - -namespace bg = boost::geometry; -typedef bg::model::d2::point_xy bg_point; -typedef bg::model::polygon bg_polygon; - -namespace -{ -namespace bg = boost::geometry; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -std::vector convertEulerAngleToMonotonic(const std::vector & angle) -{ - if (angle.empty()) { - return std::vector{}; - } - - std::vector monotonic_angle{angle.front()}; - for (size_t i = 1; i < angle.size(); ++i) { - const double diff_angle = angle.at(i) - monotonic_angle.back(); - monotonic_angle.push_back( - monotonic_angle.back() + tier4_autoware_utils::normalizeRadian(diff_angle)); - } - - return monotonic_angle; -} - -std::vector calcEuclidDist(const std::vector & x, const std::vector & y) -{ - if (x.size() != y.size()) { - std::cerr << "x y vector size should be the same." << std::endl; - } - - std::vector dist_v; - dist_v.push_back(0.0); - for (unsigned int i = 0; i < x.size() - 1; ++i) { - const double dx = x.at(i + 1) - x.at(i); - const double dy = y.at(i + 1) - y.at(i); - dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); - } - - return dist_v; -} - -std::array, 3> validateTrajectoryPoints( - const std::vector & points) -{ - constexpr double epsilon = 1e-6; - - std::vector x; - std::vector y; - std::vector yaw; - for (size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < epsilon && - std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < epsilon) { - continue; - } - } - x.push_back(points[i].pose.position.x); - y.push_back(points[i].pose.position.y); - yaw.push_back(tf2::getYaw(points[i].pose.orientation)); - } - return {x, y, yaw}; -} - -std::array, 2> validatePoints( - const std::vector & points) -{ - std::vector x; - std::vector y; - for (size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < 1e-6 && - std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < 1e-6) { - continue; - } - } - x.push_back(points[i].pose.position.x); - y.push_back(points[i].pose.position.y); - } - return {x, y}; -} - -// only two points is supported -std::vector splineTwoPoints( - std::vector base_s, std::vector base_x, const double begin_diff, - const double end_diff, std::vector new_s) -{ - const double h = base_s.at(1) - base_s.at(0); - - const double c = begin_diff; - const double d = base_x.at(0); - const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); - const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); - - std::vector res; - for (const auto & s : new_s) { - const double ds = s - base_s.at(0); - res.push_back(d + (c + (b + a * ds) * ds) * ds); - } - - return res; -} -} // namespace - -namespace tier4_autoware_utils -{ -template <> -geometry_msgs::msg::Point getPoint(const ReferencePoint & p) -{ - return p.p; -} - -template <> -geometry_msgs::msg::Pose getPose(const ReferencePoint & p) -{ - geometry_msgs::msg::Pose pose; - pose.position = p.p; - pose.orientation = createQuaternionFromYaw(p.yaw); - return pose; -} -} // namespace tier4_autoware_utils - -namespace geometry_utils -{ -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin) -{ - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point absolute_p; - absolute_p.x = point.x * cos_yaw - point.y * sin_yaw + origin.position.x; - absolute_p.y = point.x * sin_yaw + point.y * cos_yaw + origin.position.y; - absolute_p.z = point.z; - - return absolute_p; -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) -{ - const double yaw = tier4_autoware_utils::calcAzimuthAngle(a_root, a); - return tier4_autoware_utils::createQuaternionFromYaw(yaw); -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - const double dx = (8.0 * (p3.x - p2.x) - (p4.x - p1.x)) / 12.0; - const double dy = (8.0 * (p3.y - p2.y) - (p4.y - p1.y)) / 12.0; - const double yaw = std::atan2(dy, dx); - - return tier4_autoware_utils::createQuaternionFromYaw(yaw); -} - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - const geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - const double resolution = occupancy_grid_info.resolution; - const double map_y_height = occupancy_grid_info.height; - const double map_x_width = occupancy_grid_info.width; - const double map_x_in_image_resolution = relative_p.x / resolution; - const double map_y_in_image_resolution = relative_p.y / resolution; - const double image_x = map_y_height - map_y_in_image_resolution; - const double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - geometry_msgs::msg::Point image_point; - image_point.x = image_x; - image_point.y = image_y; - return image_point; - } else { - return boost::none; - } -} - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - const double map_y_height = occupancy_grid_info.height; - const double map_x_width = occupancy_grid_info.width; - const double scale = 1 / occupancy_grid_info.resolution; - const double map_x_in_image_resolution = relative_p.x * scale; - const double map_y_in_image_resolution = relative_p.y * scale; - const double image_x = map_y_height - map_y_in_image_resolution; - const double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - image_point.x = image_x; - image_point.y = image_y; - return true; - } else { - return false; - } -} -} // namespace geometry_utils - -namespace interpolation_utils -{ -std::vector interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double offset = 0.0) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - const std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - - std::vector new_s; - for (double i = offset; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - if (new_s.empty()) { - return std::vector{}; - } - - // spline interpolation - const std::vector interpolated_x = interpolation::spline(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::spline(base_s, base_y, new_s); - for (size_t i = 0; i < interpolated_x.size(); ++i) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); ++i) { - geometry_msgs::msg::Point point; - point.x = interpolated_x[i]; - point.y = interpolated_y[i]; - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolateConnected2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double begin_yaw, const double end_yaw) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - // spline interpolation - const auto interpolated_x = - splineTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); - const auto interpolated_y = - splineTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - - const size_t front_idx = (i == interpolated_x.size() - 1) ? i - 1 : i; - const double dx = interpolated_x[front_idx + 1] - interpolated_x[front_idx]; - const double dy = interpolated_y[front_idx + 1] - interpolated_y[front_idx]; - const double yaw = std::atan2(dy, dx); - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, - const std::vector & base_yaw, const double resolution) -{ - if (base_x.empty() || base_y.empty() || base_yaw.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); - - // spline interpolation - const auto interpolated_x = interpolation::spline(base_s, base_x, new_s); - const auto interpolated_y = interpolation::spline(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::spline(base_s, monotonic_base_yaw, new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - // spline interpolation - // x = interpolated[0], y = interpolated[1], yaw = interpolated[2] - std::array, 3> interpolated = - interpolation::slerp2dFromXY(base_s, base_x, base_y, new_s); - const auto & interpolated_x = interpolated[0]; - const auto & interpolated_y = interpolated[1]; - const auto & interpolated_yaw = interpolated[2]; - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector getInterpolatedTrajectoryPoints( - const std::vector & points, - const double delta_arc_length) -{ - std::array, 3> validated_pose = validateTrajectoryPoints(points); - return interpolation_utils::interpolate2DTrajectoryPoints( - validated_pose.at(0), validated_pose.at(1), delta_arc_length); -} - -std::vector getConnectedInterpolatedPoints( - const std::vector & points, - const double delta_arc_length, const double begin_yaw, const double end_yaw) -{ - std::array, 2> validated_pose = validatePoints(points); - return interpolation_utils::interpolateConnected2DPoints( - validated_pose.at(0), validated_pose.at(1), delta_arc_length, begin_yaw, end_yaw); -} -} // namespace interpolation_utils - -namespace points_utils -{ -// functions to convert to another type of points -std::vector convertToPosesWithYawEstimation( - const std::vector points) -{ - std::vector poses; - if (points.empty()) { - return poses; - } else if (points.size() == 1) { - geometry_msgs::msg::Pose pose; - pose.position = points.at(0); - poses.push_back(pose); - return poses; - } - - for (size_t i = 0; i < points.size(); ++i) { - geometry_msgs::msg::Pose pose; - pose.position = points.at(i); - - const size_t front_idx = (i == points.size() - 1) ? i - 1 : i; - const double points_yaw = - tier4_autoware_utils::calcAzimuthAngle(points.at(front_idx), points.at(front_idx + 1)); - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(points_yaw); - - poses.push_back(pose); - } - return poses; -} - -template -ReferencePoint convertToReferencePoint(const T & point) -{ - ReferencePoint ref_point; - - const auto & pose = tier4_autoware_utils::getPose(point); - ref_point.p = pose.position; - ref_point.yaw = tf2::getYaw(pose.orientation); - - return ref_point; -} - -template ReferencePoint convertToReferencePoint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & point); -template ReferencePoint convertToReferencePoint( - const geometry_msgs::msg::Pose & point); -template <> -ReferencePoint convertToReferencePoint(const geometry_msgs::msg::Point & point) -{ - ReferencePoint ref_point; - - ref_point.p = point; - - return ref_point; -} - -std::vector concatTrajectory( - const std::vector & traj_points, - const std::vector & extended_traj_points) -{ - std::vector trajectory; - trajectory.insert(trajectory.end(), traj_points.begin(), traj_points.end()); - trajectory.insert(trajectory.end(), extended_traj_points.begin(), extended_traj_points.end()); - return trajectory; -} - -void compensateLastPose( - const autoware_auto_planning_msgs::msg::PathPoint & last_path_point, - std::vector & traj_points, - const double delta_dist_threshold, const double delta_yaw_threshold) -{ - if (traj_points.empty()) { - traj_points.push_back(convertToTrajectoryPoint(last_path_point)); - return; - } - - const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; - - const double dist = - tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); - const double norm_diff_yaw = [&]() { - const double diff_yaw = - tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return tier4_autoware_utils::normalizeRadian(diff_yaw); - }(); - if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { - traj_points.push_back(convertToTrajectoryPoint(last_path_point)); - } -} - -int getNearestIdx( - const std::vector & points, const double target_s, const int begin_idx) -{ - double nearest_delta_s = std::numeric_limits::max(); - int nearest_idx = begin_idx; - for (size_t i = begin_idx; i < points.size(); i++) { - double diff = std::fabs(target_s - points[i].s); - if (diff < nearest_delta_s) { - nearest_delta_s = diff; - nearest_idx = i; - } - } - return nearest_idx; -} -} // namespace points_utils - -namespace utils -{ -void logOSQPSolutionStatus(const int solution_status, const std::string & msg) -{ - /****************** - * Solver Status * - ******************/ - const int LOCAL_OSQP_SOLVED = 1; - const int LOCAL_OSQP_SOLVED_INACCURATE = 2; - const int LOCAL_OSQP_MAX_ITER_REACHED = -2; - const int LOCAL_OSQP_PRIMAL_INFEASIBLE = -3; - const int LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE = 3; - const int LOCAL_OSQP_DUAL_INFEASIBLE = -4; - const int LOCAL_OSQP_DUAL_INFEASIBLE_INACCURATE = 4; - const int LOCAL_OSQP_SIGINT = -5; - const int LOCAL_OSQP_TIME_LIMIT_REACHED = -6; - const int LOCAL_OSQP_UNSOLVED = -10; - const int LOCAL_OSQP_NON_CVX = -7; - - if (solution_status == LOCAL_OSQP_SOLVED) { - } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] %s OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE", msg.c_str()); - } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] %s OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE", msg.c_str()); - } else if (solution_status == LOCAL_OSQP_SOLVED_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_SOLVED_INACCURATE", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_MAX_ITER_REACHED) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_ITER_REACHED", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_PRIMAL_INFEASIBLE", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_DUAL_INFEASIBLE", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_SIGINT) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_SIGINT", msg.c_str()); - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] Interrupted by user, process will be finished."); - std::exit(0); - } else if (solution_status == LOCAL_OSQP_TIME_LIMIT_REACHED) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_TIME_LIMIT_REACHED", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_UNSOLVED) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_UNSOLVED", - msg.c_str()); - } else if (solution_status == LOCAL_OSQP_NON_CVX) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_NON_CVX", msg.c_str()); - } else { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: Not defined %d", - msg.c_str(), solution_status); - } -} -} // namespace utils - -namespace -{ -geometry_msgs::msg::Point getStartPoint( - const std::vector & bound, const geometry_msgs::msg::Point & point) -{ - const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); - const auto & curr_seg_point = bound.at(segment_idx); - const auto & next_seg_point = bound.at(segment_idx); - const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; - const Eigen::Vector2d first_to_second{ - next_seg_point.x - curr_seg_point.x, next_seg_point.y - curr_seg_point.y}; - const double length = first_to_target.dot(first_to_second.normalized()); - - if (length < 0.0) { - return bound.front(); - } - - const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); - - if (first_point) { - return *first_point; - } - - return bound.front(); -} - -bool isFrontDrivableArea( - const geometry_msgs::msg::Point & point, - const std::vector & left_bound, - const std::vector & right_bound) -{ - if (left_bound.empty() || right_bound.empty()) { - return false; - } - - constexpr double min_dist = 0.1; - const auto left_start_point = getStartPoint(left_bound, right_bound.front()); - const auto right_start_point = getStartPoint(right_bound, left_bound.front()); - - // ignore point behind of the front line - const std::vector front_bound = {left_start_point, right_start_point}; - const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); - if (lat_dist_to_front_bound < min_dist) { - return true; - } - - return false; -} - -bg_polygon createDrivablePolygon( - const std::vector & left_bound, - const std::vector & right_bound) -{ - bg_polygon drivable_area_poly; - - // left bound - for (const auto lp : left_bound) { - drivable_area_poly.outer().push_back(bg_point(lp.x, lp.y)); - } - - // right bound - auto reversed_right_bound = right_bound; - std::reverse(reversed_right_bound.begin(), reversed_right_bound.end()); - for (const auto rp : reversed_right_bound) { - drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); - } - - drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); - return drivable_area_poly; -} -} // namespace - -namespace drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector & left_bound, - const std::vector & right_bound, const VehicleParam & vehicle_param, - const bool is_considering_footprint_edges) -{ - if (left_bound.empty() || right_bound.empty()) { - return false; - } - - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - const auto top_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) - .position; - const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) - .position; - const auto bottom_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) - .position; - const auto bottom_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) - .position; - - if (is_considering_footprint_edges) { - LinearRing2d footprint_polygon; - LineString2d left_bound_line; - LineString2d right_bound_line; - LineString2d back_bound_line; - - footprint_polygon.push_back({top_left_pos.x, top_left_pos.y}); - footprint_polygon.push_back({top_right_pos.x, top_right_pos.y}); - footprint_polygon.push_back({bottom_right_pos.x, bottom_right_pos.y}); - footprint_polygon.push_back({bottom_left_pos.x, bottom_left_pos.y}); - - bg::correct(footprint_polygon); - - for (const auto & p : left_bound) { - left_bound_line.push_back({p.x, p.y}); - } - - for (const auto & p : right_bound) { - right_bound_line.push_back({p.x, p.y}); - } - - back_bound_line = {left_bound_line.back(), right_bound_line.back()}; - - if ( - bg::intersects(footprint_polygon, left_bound_line) || - bg::intersects(footprint_polygon, right_bound_line) || - bg::intersects(footprint_polygon, back_bound_line)) { - return true; - } - return false; - } - - const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound); - const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound); - const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound); - const bool front_bottom_right = isFrontDrivableArea(bottom_right_pos, left_bound, right_bound); - - // create rectangle - const auto drivable_area_poly = createDrivablePolygon(left_bound, right_bound); - - if ( - !front_top_left && !bg::within(bg_point(top_left_pos.x, top_left_pos.y), drivable_area_poly)) { - return true; - } - - if ( - !front_top_right && - !bg::within(bg_point(top_right_pos.x, top_right_pos.y), drivable_area_poly)) { - return true; - } - - if ( - !front_bottom_left && - !bg::within(bg_point(bottom_left_pos.x, bottom_left_pos.y), drivable_area_poly)) { - return true; - } - - if ( - !front_bottom_right && - !bg::within(bg_point(bottom_right_pos.x, bottom_right_pos.y), drivable_area_poly)) { - return true; - } - - return false; -} -} // namespace drivable_area_utils diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index e752c31f242da..19ff1775a1211 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 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. @@ -18,50 +18,24 @@ #include #include -KinematicsBicycleModel::KinematicsBicycleModel(const double wheel_base, const double steer_limit) -: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheel_base, steer_limit) +KinematicsBicycleModel::KinematicsBicycleModel(const double wheelbase, const double steer_limit) +: VehicleModelInterface(2, 1, 2, wheelbase, steer_limit) { } void KinematicsBicycleModel::calculateStateEquationMatrix( - Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double curvature, + const double ds) const { - // const double epsilon = std::numeric_limits::epsilon(); - // constexpr double dt = 0.03; // assuming delta time for steer tau - - /* - const double lf = wheel_base_ - center_offset_from_base_; - const double lr = center_offset_from_base_; - */ - - const double delta_r = std::atan(wheel_base_ * curvature_); + const double delta_r = std::atan(wheelbase_ * curvature); const double cropped_delta_r = std::clamp(delta_r, -steer_limit_, steer_limit_); - // NOTE: cos(delta_r) will not be zero since that happens only when curvature is infinity - Ad << 1.0, ds, // - 0.0, 1.0; - - Bd << 0.0, ds / wheel_base_ / std::pow(std::cos(delta_r), 2.0); + // NOTE: cos(delta_r) will not be zero since curvature will not be infinity + Ad << 1.0, ds, 0.0, 1.0; - Wd << 0.0, -ds * curvature_ + ds / wheel_base_ * - (std::tan(cropped_delta_r) - - cropped_delta_r / std::pow(std::cos(cropped_delta_r), 2.0)); -} + Bd << 0.0, ds / wheelbase_ / std::pow(std::cos(delta_r), 2.0); -void KinematicsBicycleModel::calculateObservationMatrix(Eigen::MatrixXd & Cd) -{ - Cd << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0; -} - -void KinematicsBicycleModel::calculateObservationSparseMatrix( - std::vector> & Cd_vec) -{ - Cd_vec.clear(); - Cd_vec.push_back({0, 0, 1.0}); - Cd_vec.push_back({1, 1, 1.0}); -} - -void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd * Uref) -{ - (*Uref)(0, 0) = std::atan(wheel_base_ * curvature_); + Wd << 0.0, -ds * curvature + ds / wheelbase_ * + (std::tan(cropped_delta_r) - + cropped_delta_r / std::pow(std::cos(cropped_delta_r), 2.0)); } diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp deleted file mode 100644 index 3e317515c0d7e..0000000000000 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2020 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 "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" - -KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( - const double & wheelbase, const double & steer_lim) -: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2) -{ - wheelbase_ = wheelbase; - steer_lim_ = steer_lim; -} - -void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) -{ - auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; - - /* Linearize delta around delta_r (reference delta) */ - double delta_r = atan(wheelbase_ * curvature_); - if (abs(delta_r) >= steer_lim_) { - delta_r = steer_lim_ * static_cast(sign(delta_r)); - } - double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); - - *Ad << 1.0, ds, 0.0, 1.0; - - *Bd << 0.0, ds / wheelbase_ * cos_delta_r_squared_inv; - - *Cd << 1.0, 0.0, 0.0, 1.0; - - // Wd << 0.0, -ds / wheelbase_ * delta_r * cos_delta_r_squared_inv; - *Wd << 0.0, (-ds * delta_r) / (wheelbase_ * cos_delta_r_squared_inv); -} - -void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd * Uref) -{ - (*Uref)(0, 0) = std::atan(wheelbase_ * curvature_); -} diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp index 8e6522725c133..35da4a2eab467 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2023 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. @@ -15,23 +15,12 @@ #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" VehicleModelInterface::VehicleModelInterface( - int dim_x, int dim_u, int dim_y, double wheel_base, double steer_limit) -: dim_x_(dim_x), - dim_u_(dim_u), - dim_y_(dim_y), - wheel_base_(wheel_base), - steer_limit_(steer_limit), - center_offset_from_base_(0.0) + const int dim_x, const int dim_u, const int dim_y, const double wheelbase, + const double steer_limit) +: dim_x_(dim_x), dim_u_(dim_u), dim_y_(dim_y), wheelbase_(wheelbase), steer_limit_(steer_limit) { } -int VehicleModelInterface::getDimX() { return dim_x_; } -int VehicleModelInterface::getDimU() { return dim_u_; } -int VehicleModelInterface::getDimY() { return dim_y_; } - -void VehicleModelInterface::updateCenterOffset(const double center_offset_from_base) -{ - center_offset_from_base_ = center_offset_from_base; -} - -void VehicleModelInterface::setCurvature(const double curvature) { curvature_ = curvature; } +int VehicleModelInterface::getDimX() const { return dim_x_; } +int VehicleModelInterface::getDimU() const { return dim_u_; } +int VehicleModelInterface::getDimY() const { return dim_y_; } diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_optimizer/CMakeLists.txt index 73bb97761ba67..4731d44ed30bb 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_optimizer/CMakeLists.txt @@ -24,7 +24,7 @@ autoware_package() ament_auto_add_executable(main src/main.cpp src/static_centerline_optimizer_node.cpp - src/collision_free_optimizer_node.cpp + src/successive_trajectory_optimizer_node.cpp src/utils.cpp ) diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp new file mode 100644 index 0000000000000..57ebb947b03c2 --- /dev/null +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp @@ -0,0 +1,45 @@ +// 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. + +// NOTE: This file was copied from a part of implementation in +// https://github.com/autowarefoundation/autoware.universe/blob/main/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp + +#ifndef STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ +#define STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ + +#include "obstacle_avoidance_planner/node.hpp" +#include "static_centerline_optimizer/type_alias.hpp" + +#include +#include +#include +#include + +namespace static_centerline_optimizer +{ +class SuccessiveTrajectoryOptimizer : public obstacle_avoidance_planner::ObstacleAvoidancePlanner +{ +private: + // subscriber + rclcpp::Subscription::SharedPtr centerline_sub_; + +public: + explicit SuccessiveTrajectoryOptimizer(const rclcpp::NodeOptions & node_options); + + // subscriber callback functions + Trajectory on_centerline(const Path & path); +}; +} // namespace static_centerline_optimizer + +#endif // STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index 2bc0aa6a2aedf..7b315adbf29e8 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -19,7 +19,7 @@ @@ -56,8 +56,8 @@ - - + + diff --git a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz index 30a3776fd8526..b1834ec122f65 100644 --- a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz +++ b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz @@ -3,9 +3,12 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Raw Centerline1/Topic1 + - /DrivableArea1/Topic1 + - /Optimized Centerline1/View Footprint1 Splitter Ratio: 0.550000011920929 - Tree Height: 388 + Tree Height: 386 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -143,16 +146,55 @@ Visualization Manager: Reliability Policy: Reliable Value: /static_centerline_optimizer/input_centerline Value: true - View LaneId: - Scale: 0.10000000149011612 - Value: true - View PathWithLaneId Footprint: - Alpha: 0.5 + View Footprint: + Alpha: 1 Color: 230; 230; 50 + Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: true Vehicle Length: 4.769999980926514 Vehicle Width: 1.8300000429153442 + View LaneId: + Scale: 0.10000000149011612 + Value: true + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + - Class: rviz_plugins/PathWithLaneId + Color Border Vel Max: 3 + Enabled: true + Name: DrivableArea + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_optimizer/input_centerline + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false - Class: rviz_plugins/TrajectoryFootprint Enabled: true Name: Optimized Centerline @@ -164,14 +206,15 @@ Visualization Manager: Reliability Policy: Reliable Value: /static_centerline_optimizer/output_centerline Value: true - View Trajectory Footprint: - Alpha: 0.5 - Color: 239; 41; 41 + View Footprint: + Alpha: 1 + Color: 255; 0; 0 + Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: true Vehicle Length: 4.769999980926514 Vehicle Width: 1.8300000429153442 - View Trajectory Point: + View Point: Alpha: 1 Color: 0; 60; 255 Offset: 0 @@ -190,13 +233,20 @@ Visualization Manager: Reliability Policy: Reliable Value: /static_centerline_optimizer/debug/raw_centerline Value: false - View Path Footprint: + View Footprint: Alpha: 1 Color: 230; 230; 50 + Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: true Vehicle Length: 4.769999980926514 Vehicle Width: 1.8300000429153442 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MarkerArray @@ -362,7 +412,7 @@ Window Geometry: Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index df2e01cbcd31c..7187123c1dfd9 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -19,8 +19,8 @@ #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" #include "motion_utils/motion_utils.hpp" -#include "static_centerline_optimizer/collision_free_optimizer_node.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" +#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" @@ -391,9 +391,8 @@ std::vector StaticCenterlineOptimizerNode::plan_path( RCLCPP_INFO(get_logger(), "Converted to path and published."); // optimize trajectory by the obstacle_avoidance_planner package - CollisionFreeOptimizerNode successive_path_optimizer(create_node_options()); - const auto optimized_traj = - successive_path_optimizer.pathCallback(std::make_shared(raw_path)); + SuccessiveTrajectoryOptimizer successive_trajectory_optimizer(create_node_options()); + const auto optimized_traj = successive_trajectory_optimizer.on_centerline(raw_path); pub_optimized_centerline_->publish(optimized_traj); const auto optimized_traj_points = motion_utils::convertToTrajectoryPointArray(optimized_traj); diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp new file mode 100644 index 0000000000000..325c257e228f9 --- /dev/null +++ b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp @@ -0,0 +1,109 @@ +// 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. + +#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include +#include +#include +#include +#include + +namespace static_centerline_optimizer +{ +SuccessiveTrajectoryOptimizer::SuccessiveTrajectoryOptimizer( + const rclcpp::NodeOptions & node_options) +: ObstacleAvoidancePlanner(node_options) +{ + // subscriber + centerline_sub_ = create_subscription( + "debug/raw_centerline", rclcpp::QoS{1}.transient_local(), + std::bind(&SuccessiveTrajectoryOptimizer::on_centerline, this, std::placeholders::_1)); + + // update parameters for replan_checker to execute optimization every cycle + std::vector parameters; + parameters.push_back(rclcpp::Parameter("replan.max_path_shape_around_ego_lat_dist", 100.0)); + parameters.push_back(rclcpp::Parameter("replan.max_ego_moving_dist", 100.0)); + parameters.push_back(rclcpp::Parameter("replan.max_goal_moving_dist", 100.0)); + parameters.push_back(rclcpp::Parameter("replan.max_delta_time_sec", 0.0)); + onParam(parameters); +} + +Trajectory SuccessiveTrajectoryOptimizer::on_centerline(const Path & path) +{ + if (path.points.size() < 2) { + RCLCPP_WARN(get_logger(), "Input path size is less than 2."); + return Trajectory{}; + } + + // parameters for input path sampling + const double resample_interval = 2.0; + const double valid_optimized_path_length = 30.0; + const double path_length = motion_utils::calcArcLength(path.points); + const size_t path_segment_num = static_cast(path_length / valid_optimized_path_length); + + const auto resampled_path = motion_utils::resamplePath(path, resample_interval); // TODO(murooka) + const auto resampled_traj_points = + obstacle_avoidance_planner::trajectory_utils::convertToTrajectoryPoints(resampled_path.points); + + const size_t initial_index = 3; + std::vector whole_optimized_traj_points; + + for (size_t i = 0; i < path_segment_num; ++i) { + // calculate initial pose to start optimization + const auto initial_pose = + resampled_path.points.at(initial_index + valid_optimized_path_length / resample_interval * i) + .pose; + + // create planner data + obstacle_avoidance_planner::PlannerData planner_data; + planner_data.traj_points = resampled_traj_points; + planner_data.left_bound = path.left_bound; + planner_data.right_bound = path.right_bound; + planner_data.ego_pose = initial_pose; + + const auto optimized_traj_points = optimizeTrajectory(planner_data); + + for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { + const double dist = tier4_autoware_utils::calcDistance2d( + whole_optimized_traj_points.at(j), optimized_traj_points.front()); + if (dist < 0.5) { + const std::vector extracted_whole_optimized_traj_points{ + whole_optimized_traj_points.begin(), whole_optimized_traj_points.begin() + j - 1}; + whole_optimized_traj_points = extracted_whole_optimized_traj_points; + } + } + + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + } + } + + // resample + auto output_traj_msg = motion_utils::resampleTrajectory( + motion_utils::convertToTrajectory(whole_optimized_traj_points), 1.0); + output_traj_msg.header = path.header; + + return output_traj_msg; +} +} // namespace static_centerline_optimizer + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(static_centerline_optimizer::SuccessiveTrajectoryOptimizer) From e7089d76ae15514f16752ab23a23d2f18d341b44 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 1 Mar 2023 13:23:43 +0900 Subject: [PATCH 06/24] docs(behavior_path_planner): fix dead link (#2965) Signed-off-by: satoshi-ota --- planning/behavior_path_planner/README.md | 2 +- ...havior_path_planner_path_generation_design.md | 2 +- sensing/gnss_poser/README.md | 16 ++++++++-------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index cf335aa3a5db1..c245cdf18d263 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -167,7 +167,7 @@ If there are multiple avoidance targets and the lateral distances of these are c The path generation is computed in Frenet coordinates. The shift length profile for avoidance is generated by four segmental constant jerk polynomials, and added to the original path. Since the lateral jerk can be approximately seen as a steering maneuver, this calculation yields a result similar to a Clothoid curve. -For more detail, see [behavior-path-planner-path-generation](./behavior_path_planner_path_generation.md). +For more detail, see [behavior-path-planner-path-generation](./behavior_path_planner_path_generation_design.md). diff --git a/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md b/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md index 550d37a4a4ea1..e73bb11a77ba5 100644 --- a/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md +++ b/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md @@ -1,6 +1,6 @@ # Path Generation design -This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp). +This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp). ## Overview diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index a0af9623ab90c..6deb472cd3362 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -27,14 +27,14 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------- | ------ | ---------------- | ------------------------------------------------------------------------------------------------------------------------------------------ | -| `base_frame` | string | "base_link" | frame id | -| `gnss_frame` | string | "gnss" | frame id | -| `gnss_base_frame` | string | "gnss_base_link" | frame id | -| `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | -| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. [click here for more details](https://www.gsi.go.jp/LAW/heimencho.html) | +| Name | Type | Default Value | Description | +| ------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------------------------- | +| `base_frame` | string | "base_link" | frame id | +| `gnss_frame` | string | "gnss" | frame id | +| `gnss_base_frame` | string | "gnss_base_link" | frame id | +| `map_frame` | string | "map" | frame id | +| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | +| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. | ## Assumptions / Known limits From 5f75c7d79a5212cf709a5b8b3bea6061e46c3203 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 1 Mar 2023 15:51:36 +0900 Subject: [PATCH 07/24] fix(motion_velocity_smoother): fix external velocity limit if ego vehicle is stopped (#2959) Signed-off-by: Fumiya Watanabe --- .../src/motion_velocity_smoother_node.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 8647945c4caa5..8531fec2c5764 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -304,6 +304,13 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit() constexpr double eps = 1.0E-04; const double margin = node_param_.margin_to_insert_external_velocity_limit; + // Set distance as zero if ego vehicle is stopped and external velocity limit is zero + if ( + std::fabs(current_odometry_ptr_->twist.twist.linear.x) < eps && + external_velocity_limit_.velocity < eps) { + external_velocity_limit_.dist = 0.0; + } + // calculate distance and maximum velocity // to decelerate to external velocity limit with jerk and acceleration // constraints. From a2c62dcef31f162ef41b9f3f03bfbeee88ecc03b Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 1 Mar 2023 22:36:34 +0900 Subject: [PATCH 08/24] fix(fault injection): add find package to cmake (#2973) * fix(fault injection) add find package to cmake Signed-off-by: tomoya.kimura * feat: add pluginlib to dependency Signed-off-by: tomoya.kimura --------- Signed-off-by: tomoya.kimura --- simulator/fault_injection/CMakeLists.txt | 2 ++ simulator/fault_injection/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/simulator/fault_injection/CMakeLists.txt b/simulator/fault_injection/CMakeLists.txt index 0bc1663412960..11bf03facd67f 100644 --- a/simulator/fault_injection/CMakeLists.txt +++ b/simulator/fault_injection/CMakeLists.txt @@ -4,6 +4,8 @@ project(fault_injection) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(pluginlib REQUIRED) + ament_auto_add_library(fault_injection_node_component SHARED src/fault_injection_node/fault_injection_node.cpp ) diff --git a/simulator/fault_injection/package.xml b/simulator/fault_injection/package.xml index 95e50e6448807..3b3cd5ca6c45b 100644 --- a/simulator/fault_injection/package.xml +++ b/simulator/fault_injection/package.xml @@ -14,6 +14,7 @@ diagnostic_aggregator diagnostic_msgs diagnostic_updater + pluginlib rclcpp rclcpp_components tier4_autoware_utils From f76977af9fb53e3f0df452d9efd3c715141cfed5 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 2 Mar 2023 04:09:10 +0900 Subject: [PATCH 09/24] feat(tier4_planning_rviz_plugin): move footprint plugin to path (#2971) * feat(tier4_rviz_plugin): simplify tier4_planning_rviz_plugin Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../tier4_planning_rviz_plugin/CMakeLists.txt | 3 - .../include/path/display.hpp | 19 + .../include/path/display_base.hpp | 436 +++++++++++++----- .../include/path_footprint/display.hpp | 67 --- .../include/path_footprint/display_base.hpp | 315 ------------- .../plugins/plugin_description.xml | 15 - .../src/path/display.cpp | 71 +++ .../src/path_footprint/display.cpp | 97 ---- 8 files changed, 409 insertions(+), 614 deletions(-) delete mode 100644 common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp delete mode 100644 common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp delete mode 100644 common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index bf18e078a7604..5c17af5df22d1 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -22,9 +22,6 @@ ament_auto_add_library(tier4_planning_rviz_plugin SHARED include/path/display.hpp src/path/display.cpp # footprint - include/path_footprint/display_base.hpp - include/path_footprint/display.hpp - src/path_footprint/display.cpp include/pose_with_uuid_stamped/display.hpp src/pose_with_uuid_stamped/display.cpp include/mission_checkpoint/mission_checkpoint.hpp diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp index ffcc5a6b63638..7d9bde57a49c0 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display.hpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include namespace @@ -158,6 +160,23 @@ class AutowarePathWithLaneIdDisplay : public AutowarePathWithDrivableAreaDisplay { Q_OBJECT +public: + AutowarePathWithLaneIdDisplay(); + ~AutowarePathWithLaneIdDisplay(); + +private: + void preprocessMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + void processMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) override; + + rviz_common::properties::BoolProperty property_lane_id_view_; + rviz_common::properties::FloatProperty property_lane_id_scale_; + + using LaneIdObject = + std::pair, std::unique_ptr>; + std::vector lane_id_obj_ptrs_; }; class AutowarePathDisplay diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 028e9779e1415..86c6201b81d51 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -95,6 +96,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) namespace rviz_plugins { +using vehicle_info_util::VehicleInfo; +using vehicle_info_util::VehicleInfoUtil; template class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay { @@ -115,7 +118,21 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay property_velocity_color_{"Color", Qt::black, "", &property_velocity_view_}, // velocity text property_velocity_text_view_{"View Text Velocity", false, "", this}, - property_velocity_text_scale_{"Scale", 0.3, "", &property_velocity_text_view_} + property_velocity_text_scale_{"Scale", 0.3, "", &property_velocity_text_view_}, + // footprint + property_footprint_view_{"View Footprint", false, "", this}, + property_footprint_alpha_{"Alpha", 1.0, "", &property_footprint_view_}, + property_footprint_color_{"Color", QColor(230, 230, 50), "", &property_footprint_view_}, + property_vehicle_length_{"Vehicle Length", 4.77, "", &property_footprint_view_}, + property_vehicle_width_{"Vehicle Width", 1.83, "", &property_footprint_view_}, + property_rear_overhang_{"Rear Overhang", 1.03, "", &property_footprint_view_}, + property_offset_{"Offset from BaseLink", 0.0, "", &property_footprint_view_}, + // point + property_point_view_{"View Point", false, "", this}, + property_point_alpha_{"Alpha", 1.0, "", &property_point_view_}, + property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_}, + property_point_radius_{"Radius", 0.1, "", &property_point_view_}, + property_point_offset_{"Offset", 0.0, "", &property_point_view_} { // path property_path_width_.setMin(0.0); @@ -128,6 +145,18 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // velocity text property_velocity_scale_.setMin(0.1); property_velocity_scale_.setMax(10.0); + + // initialize footprint + property_footprint_alpha_.setMin(0.0); + property_footprint_alpha_.setMax(1.0); + property_vehicle_length_.setMin(0.0); + property_vehicle_width_.setMin(0.0); + property_rear_overhang_.setMin(0.0); + // initialize point + property_point_alpha_.setMin(0.0); + property_point_alpha_.setMax(1.0); + + updateVehicleInfo(); } ~AutowarePathBaseDisplay() override @@ -141,6 +170,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); this->scene_manager_->destroySceneNode(node); } + this->scene_manager_->destroyManualObject(footprint_manual_object_); + this->scene_manager_->destroyManualObject(point_manual_object_); } } void onInitialize() override @@ -149,11 +180,18 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay path_manual_object_ = this->scene_manager_->createManualObject(); velocity_manual_object_ = this->scene_manager_->createManualObject(); + footprint_manual_object_ = this->scene_manager_->createManualObject(); + point_manual_object_ = this->scene_manager_->createManualObject(); path_manual_object_->setDynamic(true); velocity_manual_object_->setDynamic(true); + footprint_manual_object_->setDynamic(true); + point_manual_object_->setDynamic(true); this->scene_node_->attachObject(path_manual_object_); this->scene_node_->attachObject(velocity_manual_object_); + this->scene_node_->attachObject(footprint_manual_object_); + this->scene_node_->attachObject(point_manual_object_); } + void reset() override { rviz_common::MessageFilterDisplay::MFDClass::reset(); @@ -167,10 +205,18 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay } velocity_text_nodes_.clear(); velocity_texts_.clear(); + footprint_manual_object_->clear(); + point_manual_object_->clear(); } protected: virtual void visualizeDrivableArea([[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) {} + virtual void preprocessMessageDetail([[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) {} + virtual void processMessageDetail( + [[maybe_unused]] const typename T::ConstSharedPtr msg_ptr, [[maybe_unused]] const size_t p_idx) + { + } + void processMessage(const typename T::ConstSharedPtr msg_ptr) override { if (!validateFloats(msg_ptr)) { @@ -188,10 +234,27 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), qPrintable(this->fixed_frame_)); } - this->scene_node_->setPosition(position); this->scene_node_->setOrientation(orientation); + // visualize Path + visualizePath(msg_ptr); + + // visualize drivable area + visualizeDrivableArea(msg_ptr); + + // visualize PathFootprint + visualizePathFootprint(msg_ptr); + + last_msg_ptr_ = msg_ptr; + } + + void visualizePath(const typename T::ConstSharedPtr msg_ptr) + { + if (msg_ptr->points.empty()) { + return; + } + path_manual_object_->clear(); velocity_manual_object_->clear(); @@ -200,140 +263,237 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); material->setDepthWriteEnabled(false); - if (!msg_ptr->points.empty()) { - path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); - velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); - path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - // path_manual_object_->begin("BaseWhiteNoLighting", - // Ogre::RenderOperation::OT_TRIANGLE_STRIP); - velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); - - if (msg_ptr->points.size() > velocity_texts_.size()) { - for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { - Ogre::SceneNode * node = this->scene_node_->createChildSceneNode(); - rviz_rendering::MovableText * text = - new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); - text->setVisible(false); - text->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node->attachObject(text); - velocity_texts_.push_back(text); - velocity_text_nodes_.push_back(node); + path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); + velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); + path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + + if (msg_ptr->points.size() > velocity_texts_.size()) { + for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { + Ogre::SceneNode * node = this->scene_node_->createChildSceneNode(); + rviz_rendering::MovableText * text = + new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); + text->setVisible(false); + text->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node->attachObject(text); + velocity_texts_.push_back(text); + velocity_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < velocity_texts_.size()) { + for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + velocity_texts_.resize(msg_ptr->points.size()); + velocity_text_nodes_.resize(msg_ptr->points.size()); + } + + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { + const auto & path_point = msg_ptr->points.at(point_idx); + const auto & pose = tier4_autoware_utils::getPose(path_point); + const auto & velocity = tier4_autoware_utils::getLongitudinalVelocity(path_point); + + // path + if (property_path_view_.getBool()) { + Ogre::ColourValue color; + if (property_path_color_view_.getBool()) { + color = rviz_common::properties::qtToOgre(property_path_color_.getColor()); + } else { + // color change depending on velocity + std::unique_ptr dynamic_color_ptr = + setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + color = *dynamic_color_ptr; + } + color.a = property_path_alpha_.getFloat(); + Eigen::Vector3f vec_in; + Eigen::Vector3f vec_out; + Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); + { + vec_in << 0, (property_path_width_.getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + if (!isDrivingForward(msg_ptr->points, point_idx)) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + static_cast(pose.position.x) + vec_out.x(), + static_cast(pose.position.y) + vec_out.y(), + static_cast(pose.position.z) + vec_out.z()); + path_manual_object_->colour(color); } - } else if (msg_ptr->points.size() < velocity_texts_.size()) { - for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->detachAllObjects(); - node->removeAndDestroyAllChildren(); - this->scene_manager_->destroySceneNode(node); + { + vec_in << 0, -(property_path_width_.getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + if (!isDrivingForward(msg_ptr->points, point_idx)) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + static_cast(pose.position.x) + vec_out.x(), + static_cast(pose.position.y) + vec_out.y(), + static_cast(pose.position.z) + vec_out.z()); + path_manual_object_->colour(color); } - velocity_texts_.resize(msg_ptr->points.size()); - velocity_text_nodes_.resize(msg_ptr->points.size()); } - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - const auto & pose = tier4_autoware_utils::getPose(path_point); - const auto & velocity = tier4_autoware_utils::getLongitudinalVelocity(path_point); - - // path - if (property_path_view_.getBool()) { - Ogre::ColourValue color; - if (property_path_color_view_.getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_.getColor()); - } else { - // color change depending on velocity - std::unique_ptr dynamic_color_ptr = - setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_.getFloat(); - Eigen::Vector3f vec_in; - Eigen::Vector3f vec_out; - Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); + // velocity + if (property_velocity_view_.getBool()) { + Ogre::ColourValue color; + if (property_velocity_color_view_.getBool()) { + color = rviz_common::properties::qtToOgre(property_velocity_color_.getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = + setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + color = *dynamic_color_ptr; + } + color.a = property_velocity_alpha_.getFloat(); + + velocity_manual_object_->position( + pose.position.x, pose.position.y, + static_cast(pose.position.z) + velocity * property_velocity_scale_.getFloat()); + velocity_manual_object_->colour(color); + } + + // velocity text + if (property_velocity_text_view_.getBool()) { + Ogre::Vector3 position; + position.x = pose.position.x; + position.y = pose.position.y; + position.z = pose.position.z; + Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + const double vel = velocity; + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << vel; + text->setCaption(ss.str()); + text->setCharacterHeight(property_velocity_text_scale_.getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + text->setVisible(false); + } + } + + path_manual_object_->end(); + velocity_manual_object_->end(); + } + + void visualizePathFootprint(const typename T::ConstSharedPtr msg_ptr) + { + if (msg_ptr->points.empty()) { + return; + } + + footprint_manual_object_->clear(); + point_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); + footprint_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + + point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); + point_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); + + const float offset_from_baselink = property_offset_.getFloat(); + + preprocessMessageDetail(msg_ptr); + + for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { + const auto & point = msg_ptr->points.at(p_idx); + const auto & pose = tier4_autoware_utils::getPose(point); + // footprint + if (property_footprint_view_.getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_footprint_color_.getColor()); + color.a = property_footprint_alpha_.getFloat(); + + const auto info = vehicle_footprint_info_; + const float top = info->length - info->rear_overhang - offset_from_baselink; + const float bottom = -info->rear_overhang + offset_from_baselink; + const float left = -info->width / 2.0; + const float right = info->width / 2.0; + + const std::array lon_offset_vec{top, top, bottom, bottom}; + const std::array lat_offset_vec{left, right, right, left}; + + for (int f_idx = 0; f_idx < 4; ++f_idx) { + const Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + { - vec_in << 0, (property_path_width_.getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - static_cast(pose.position.x) + vec_out.x(), - static_cast(pose.position.y) + vec_out.y(), - static_cast(pose.position.z) + vec_out.z()); - path_manual_object_->colour(color); + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; + const auto offset_to_edge = quat * offset_vec; + footprint_manual_object_->position( + pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), + pose.position.z); + footprint_manual_object_->colour(color); } { - vec_in << 0, -(property_path_width_.getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - static_cast(pose.position.x) + vec_out.x(), - static_cast(pose.position.y) + vec_out.y(), - static_cast(pose.position.z) + vec_out.z()); - path_manual_object_->colour(color); + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; + const auto offset_to_edge = quat * offset_vec; + footprint_manual_object_->position( + pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), + pose.position.z); + footprint_manual_object_->colour(color); } } + } - // velocity - if (property_velocity_view_.getBool()) { - Ogre::ColourValue color; - if (property_velocity_color_view_.getBool()) { - color = rviz_common::properties::qtToOgre(property_velocity_color_.getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = - setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); - color = *dynamic_color_ptr; - } - color.a = property_velocity_alpha_.getFloat(); - - velocity_manual_object_->position( - pose.position.x, pose.position.y, - static_cast(pose.position.z) + velocity * property_velocity_scale_.getFloat()); - velocity_manual_object_->colour(color); - } - - // velocity text - if (property_velocity_text_view_.getBool()) { - Ogre::Vector3 position; - position.x = pose.position.x; - position.y = pose.position.y; - position.z = pose.position.z; - Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); - node->setPosition(position); - - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - const double vel = velocity; - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << vel; - text->setCaption(ss.str()); - text->setCharacterHeight(property_velocity_text_scale_.getFloat()); - text->setVisible(true); - } else { - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - text->setVisible(false); + // point + if (property_point_view_.getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_point_color_.getColor()); + color.a = property_point_alpha_.getFloat(); + + const double offset = property_point_offset_.getFloat(); + const double yaw = tf2::getYaw(pose.orientation); + const double base_x = pose.position.x + offset * std::cos(yaw); + const double base_y = pose.position.y + offset * std::sin(yaw); + const double base_z = pose.position.z; + + const double radius = property_point_radius_.getFloat(); + for (size_t s_idx = 0; s_idx < 8; ++s_idx) { + const double current_angle = static_cast(s_idx) / 8.0 * 2.0 * M_PI; + const double next_angle = static_cast(s_idx + 1) / 8.0 * 2.0 * M_PI; + point_manual_object_->position( + base_x + radius * std::cos(current_angle), base_y + radius * std::sin(current_angle), + base_z); + point_manual_object_->colour(color); + + point_manual_object_->position( + base_x + radius * std::cos(next_angle), base_y + radius * std::sin(next_angle), base_z); + point_manual_object_->colour(color); + + point_manual_object_->position(base_x, base_y, base_z); + point_manual_object_->colour(color); } } - path_manual_object_->end(); - velocity_manual_object_->end(); + processMessageDetail(msg_ptr, p_idx); } - // visualize drivable area - visualizeDrivableArea(msg_ptr); - - last_msg_ptr_ = msg_ptr; + footprint_manual_object_->end(); + point_manual_object_->end(); } Ogre::ManualObject * path_manual_object_{nullptr}; Ogre::ManualObject * velocity_manual_object_{nullptr}; + Ogre::ManualObject * footprint_manual_object_{nullptr}; + Ogre::ManualObject * point_manual_object_{nullptr}; std::vector velocity_texts_; std::vector velocity_text_nodes_; @@ -351,8 +511,50 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_common::properties::BoolProperty property_velocity_text_view_; rviz_common::properties::FloatProperty property_velocity_text_scale_; + rviz_common::properties::BoolProperty property_footprint_view_; + rviz_common::properties::FloatProperty property_footprint_alpha_; + rviz_common::properties::ColorProperty property_footprint_color_; + rviz_common::properties::FloatProperty property_vehicle_length_; + rviz_common::properties::FloatProperty property_vehicle_width_; + rviz_common::properties::FloatProperty property_rear_overhang_; + rviz_common::properties::FloatProperty property_offset_; + + rviz_common::properties::BoolProperty property_point_view_; + rviz_common::properties::FloatProperty property_point_alpha_; + rviz_common::properties::ColorProperty property_point_color_; + rviz_common::properties::FloatProperty property_point_radius_; + rviz_common::properties::FloatProperty property_point_offset_; + private: typename T::ConstSharedPtr last_msg_ptr_; + + struct VehicleFootprintInfo + { + VehicleFootprintInfo(const float l, const float w, const float r) + : length(l), width(w), rear_overhang(r) + { + } + float length, width, rear_overhang; + }; + + std::shared_ptr vehicle_info_; + std::shared_ptr vehicle_footprint_info_; + + void updateVehicleInfo() + { + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + const float length{property_vehicle_length_.getFloat()}; + const float width{property_vehicle_width_.getFloat()}; + const float rear_overhang{property_rear_overhang_.getFloat()}; + + vehicle_footprint_info_ = + std::make_shared(length, width, rear_overhang); + } + } }; } // namespace rviz_plugins diff --git a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp deleted file mode 100644 index 1db170dd0cd46..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// 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 PATH_FOOTPRINT__DISPLAY_HPP_ -#define PATH_FOOTPRINT__DISPLAY_HPP_ - -#include -#include - -#include -#include -#include - -#include -#include -#include - -namespace rviz_plugins -{ -class AutowarePathWithLaneIdFootprintDisplay -: public AutowarePathFootBaseprintDisplay -{ - Q_OBJECT - -public: - AutowarePathWithLaneIdFootprintDisplay(); - -private: - void resetDetail() override; - void preprocessMessageDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; - void processMessageDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, - const size_t p_idx) override; - - rviz_common::properties::BoolProperty property_lane_id_view_; - rviz_common::properties::FloatProperty property_lane_id_scale_; - - using LaneIdObject = - std::pair, std::unique_ptr>; - std::vector lane_id_obj_ptrs_; -}; -class AutowarePathFootprintDisplay -: public AutowarePathFootBaseprintDisplay -{ - Q_OBJECT -}; - -class AutowareTrajectoryFootprintDisplay -: public AutowarePathFootBaseprintDisplay -{ - Q_OBJECT -}; -} // namespace rviz_plugins - -#endif // PATH_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp deleted file mode 100644 index 1b83b7c3258f3..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp +++ /dev/null @@ -1,315 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// 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 PATH_FOOTPRINT__DISPLAY_BASE_HPP_ -#define PATH_FOOTPRINT__DISPLAY_BASE_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -namespace rviz_plugins -{ -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; - -template -class AutowarePathFootBaseprintDisplay : public rviz_common::MessageFilterDisplay -{ -public: - AutowarePathFootBaseprintDisplay() - : // footprint - property_footprint_view_{"View Footprint", true, "", this}, - property_footprint_alpha_{"Alpha", 1.0, "", &property_footprint_view_}, - property_footprint_color_{"Color", QColor(230, 230, 50), "", &property_footprint_view_}, - property_vehicle_length_{"Vehicle Length", 4.77, "", &property_footprint_view_}, - property_vehicle_width_{"Vehicle Width", 1.83, "", &property_footprint_view_}, - property_rear_overhang_{"Rear Overhang", 1.03, "", &property_footprint_view_}, - property_offset_{"Offset from BaseLink", 0.0, "", &property_footprint_view_}, - // point - property_point_view_{"View Point", false, "", this}, - property_point_alpha_{"Alpha", 1.0, "", &property_point_view_}, - property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_}, - property_point_radius_{"Radius", 0.1, "", &property_point_view_}, - property_point_offset_{"Offset", 0.0, "", &property_point_view_} - { - // initialize footprint - property_footprint_alpha_.setMin(0.0); - property_footprint_alpha_.setMax(1.0); - property_vehicle_length_.setMin(0.0); - property_vehicle_width_.setMin(0.0); - property_rear_overhang_.setMin(0.0); - // initialize point - property_point_alpha_.setMin(0.0); - property_point_alpha_.setMax(1.0); - - updateVehicleInfo(); - } - - virtual ~AutowarePathFootBaseprintDisplay() - { - if (this->initialized()) { - this->scene_manager_->destroyManualObject(footprint_manual_object_); - this->scene_manager_->destroyManualObject(point_manual_object_); - } - } - - void onInitialize() override - { - rviz_common::MessageFilterDisplay::MFDClass::onInitialize(); - - footprint_manual_object_ = this->scene_manager_->createManualObject(); - footprint_manual_object_->setDynamic(true); - this->scene_node_->attachObject(footprint_manual_object_); - - point_manual_object_ = this->scene_manager_->createManualObject(); - point_manual_object_->setDynamic(true); - this->scene_node_->attachObject(point_manual_object_); - } - - void reset() override - { - rviz_common::MessageFilterDisplay::MFDClass::reset(); - footprint_manual_object_->clear(); - point_manual_object_->clear(); - - resetDetail(); - } - -protected: - virtual void resetDetail() {} - virtual void preprocessMessageDetail([[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) {} - virtual void processMessageDetail( - [[maybe_unused]] const typename T::ConstSharedPtr msg_ptr, [[maybe_unused]] const size_t p_idx) - { - } - - Ogre::ManualObject * footprint_manual_object_; - rviz_common::properties::BoolProperty property_footprint_view_; - rviz_common::properties::FloatProperty property_footprint_alpha_; - rviz_common::properties::ColorProperty property_footprint_color_; - rviz_common::properties::FloatProperty property_vehicle_length_; - rviz_common::properties::FloatProperty property_vehicle_width_; - rviz_common::properties::FloatProperty property_rear_overhang_; - rviz_common::properties::FloatProperty property_offset_; - - Ogre::ManualObject * point_manual_object_; - rviz_common::properties::BoolProperty property_point_view_; - rviz_common::properties::FloatProperty property_point_alpha_; - rviz_common::properties::ColorProperty property_point_color_; - rviz_common::properties::FloatProperty property_point_radius_; - rviz_common::properties::FloatProperty property_point_offset_; - - void updateVisualization() - { - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } - } - -private: - struct VehicleFootprintInfo - { - VehicleFootprintInfo(const float l, const float w, const float r) - : length(l), width(w), rear_overhang(r) - { - } - float length, width, rear_overhang; - }; - - std::shared_ptr vehicle_info_; - std::shared_ptr vehicle_footprint_info_; - - void updateVehicleInfo() - { - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); - } else { - const float length{property_vehicle_length_.getFloat()}; - const float width{property_vehicle_width_.getFloat()}; - const float rear_overhang{property_rear_overhang_.getFloat()}; - - vehicle_footprint_info_ = - std::make_shared(length, width, rear_overhang); - } - } - - typename T::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) - { - for (auto && point : msg_ptr->points) { - if (!rviz_common::validateFloats(tier4_autoware_utils::getPose(point))) { - return false; - } - } - return true; - } - - void processMessage(const typename T::ConstSharedPtr msg_ptr) override - { - if (!validateFloats(msg_ptr)) { - this->setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - // This doesn't work in the constructor. - if (!vehicle_info_) { - try { - vehicle_info_ = std::make_shared( - VehicleInfoUtil(*this->rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); - updateVehicleInfo(); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - this->rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *this->rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, - "Failed to get vehicle_info: %s", e.what()); - } - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!this->context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - this->rviz_ros_node_.lock()->get_raw_node()->get_logger(), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(this->fixed_frame_)); - } - - this->scene_node_->setPosition(position); - this->scene_node_->setOrientation(orientation); - - footprint_manual_object_->clear(); - point_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); - footprint_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - - point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); - point_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); - - const float offset_from_baselink = property_offset_.getFloat(); - - preprocessMessageDetail(msg_ptr); - - for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { - const auto & point = msg_ptr->points.at(p_idx); - const auto & pose = tier4_autoware_utils::getPose(point); - // footprint - if (property_footprint_view_.getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_footprint_color_.getColor()); - color.a = property_footprint_alpha_.getFloat(); - - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang - offset_from_baselink; - const float bottom = -info->rear_overhang + offset_from_baselink; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - - const std::array lon_offset_vec{top, top, bottom, bottom}; - const std::array lat_offset_vec{left, right, right, left}; - - for (int f_idx = 0; f_idx < 4; ++f_idx) { - const Eigen::Quaternionf quat( - pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; - const auto offset_to_edge = quat * offset_vec; - footprint_manual_object_->position( - pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), - pose.position.z); - footprint_manual_object_->colour(color); - } - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; - const auto offset_to_edge = quat * offset_vec; - footprint_manual_object_->position( - pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), - pose.position.z); - footprint_manual_object_->colour(color); - } - } - } - - // point - if (property_point_view_.getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_point_color_.getColor()); - color.a = property_point_alpha_.getFloat(); - - const double offset = property_point_offset_.getFloat(); - const double yaw = tf2::getYaw(pose.orientation); - const double base_x = pose.position.x + offset * std::cos(yaw); - const double base_y = pose.position.y + offset * std::sin(yaw); - const double base_z = pose.position.z; - - const double radius = property_point_radius_.getFloat(); - for (size_t s_idx = 0; s_idx < 8; ++s_idx) { - const double current_angle = static_cast(s_idx) / 8.0 * 2.0 * M_PI; - const double next_angle = static_cast(s_idx + 1) / 8.0 * 2.0 * M_PI; - point_manual_object_->position( - base_x + radius * std::cos(current_angle), base_y + radius * std::sin(current_angle), - base_z); - point_manual_object_->colour(color); - - point_manual_object_->position( - base_x + radius * std::cos(next_angle), base_y + radius * std::sin(next_angle), - base_z); - point_manual_object_->colour(color); - - point_manual_object_->position(base_x, base_y, base_z); - point_manual_object_->colour(color); - } - } - - processMessageDetail(msg_ptr, p_idx); - } - - footprint_manual_object_->end(); - point_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; - } -}; -} // namespace rviz_plugins - -#endif // PATH_FOOTPRINT__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml index adb48e990e3c8..0a80327199606 100644 --- a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml @@ -4,21 +4,11 @@ base_class_type="rviz_common::Display"> Display path points of autoware_auto_planning_msg::Path - - Display footprint of autoware_auto_planning_msg::Path - Display path_with_lane_id of autoware_auto_planning_msg::PathWithLaneId - - Display footprint of autoware_auto_planning_msg::PathWithLaneId - @@ -29,11 +19,6 @@ base_class_type="rviz_common::Display"> Display trajectory points of autoware_auto_planning_msg::Trajectory - - Display footprint of autoware_auto_planning_msg::Trajectory - diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 4e1a14de2290d..d805d004a56a3 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -15,6 +15,77 @@ #include #include +namespace rviz_plugins +{ +AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay() +: property_lane_id_view_{"View LaneId", true, "", this}, + property_lane_id_scale_{"Scale", 0.1, "", &property_lane_id_view_} +{ +} + +AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() +{ + for (const auto & e : lane_id_obj_ptrs_) { + scene_node_->removeChild(e.first.get()); + } + lane_id_obj_ptrs_.clear(); + lane_id_obj_ptrs_.shrink_to_fit(); +} + +void AutowarePathWithLaneIdDisplay::preprocessMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) +{ + const size_t size = msg_ptr->points.size(); + if (size > lane_id_obj_ptrs_.size()) { + for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) { + std::unique_ptr node_ptr; + node_ptr.reset(scene_node_->createChildSceneNode()); + auto text_ptr = + std::make_unique("not initialized", "Liberation Sans", 0.1); + text_ptr->setVisible(false); + text_ptr->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node_ptr->attachObject(text_ptr.get()); + lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr))); + } + } else { + for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) { + scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get()); + } + lane_id_obj_ptrs_.resize(size); + } +} + +void AutowarePathWithLaneIdDisplay::processMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) +{ + const auto & point = msg_ptr->points.at(p_idx); + + // LaneId + if (property_lane_id_view_.getBool()) { + Ogre::Vector3 position; + position.x = point.point.pose.position.x; + position.y = point.point.pose.position.y; + position.z = point.point.pose.position.z; + auto & node_ptr = lane_id_obj_ptrs_.at(p_idx).first; + node_ptr->setPosition(position); + + const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; + std::string lane_ids_str = ""; + for (const auto & e : point.lane_ids) { + lane_ids_str += std::to_string(e) + ", "; + } + text_ptr->setCaption(lane_ids_str); + text_ptr->setCharacterHeight(property_lane_id_scale_.getFloat()); + text_ptr->setVisible(true); + } else { + const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; + text_ptr->setVisible(false); + } +} +} // namespace rviz_plugins + PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdDisplay, rviz_common::Display) PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathDisplay, rviz_common::Display) PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp deleted file mode 100644 index 1124cde14da15..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2023 TIER IV, Inc. All rights reserved. -// -// 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 - -#define EIGEN_MPL2_ONLY -#include -#include -#include -#include - -namespace rviz_plugins -{ -AutowarePathWithLaneIdFootprintDisplay::AutowarePathWithLaneIdFootprintDisplay() -: property_lane_id_view_{"View LaneId", true, "", this}, - property_lane_id_scale_{"Scale", 0.1, "", &property_lane_id_view_} -{ -} - -void AutowarePathWithLaneIdFootprintDisplay::resetDetail() -{ - for (const auto & e : lane_id_obj_ptrs_) { - scene_node_->removeChild(e.first.get()); - } - lane_id_obj_ptrs_.clear(); - lane_id_obj_ptrs_.shrink_to_fit(); -} - -void AutowarePathWithLaneIdFootprintDisplay::preprocessMessageDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) -{ - const size_t size = msg_ptr->points.size(); - if (size > lane_id_obj_ptrs_.size()) { - for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) { - std::unique_ptr node_ptr; - node_ptr.reset(scene_node_->createChildSceneNode()); - auto text_ptr = - std::make_unique("not initialized", "Liberation Sans", 0.1); - text_ptr->setVisible(false); - text_ptr->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node_ptr->attachObject(text_ptr.get()); - lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr))); - } - } else { - for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) { - scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get()); - } - lane_id_obj_ptrs_.resize(size); - } -} - -void AutowarePathWithLaneIdFootprintDisplay::processMessageDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, - const size_t p_idx) -{ - const auto & point = msg_ptr->points.at(p_idx); - - // LaneId - if (property_lane_id_view_.getBool()) { - Ogre::Vector3 position; - position.x = point.point.pose.position.x; - position.y = point.point.pose.position.y; - position.z = point.point.pose.position.z; - auto & node_ptr = lane_id_obj_ptrs_.at(p_idx).first; - node_ptr->setPosition(position); - - const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; - std::string lane_ids_str = ""; - for (const auto & e : point.lane_ids) { - lane_ids_str += std::to_string(e) + ", "; - } - text_ptr->setCaption(lane_ids_str); - text_ptr->setCharacterHeight(property_lane_id_scale_.getFloat()); - text_ptr->setVisible(true); - } else { - const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; - text_ptr->setVisible(false); - } -} -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdFootprintDisplay, rviz_common::Display) -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathFootprintDisplay, rviz_common::Display) -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryFootprintDisplay, rviz_common::Display) From 78388cb63b5923a90506c8f8e6e2ca01ce31830c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 2 Mar 2023 08:13:23 +0900 Subject: [PATCH 10/24] feat(behavior_path_planner): be able to switch framework by macro (#2964) * feat(behavior_path_planner): be able to switch framework by macro Signed-off-by: satoshi-ota * feat(behavior_paht_planner): separate header Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- planning/behavior_path_planner/CMakeLists.txt | 9 +- .../behavior_path_planner_node.hpp | 2 +- .../behavior_path_planner/module_status.hpp | 39 +++ .../behavior_path_planner/planner_manager.hpp | 4 +- .../scene_module/scene_module_interface.hpp | 116 ++++--- .../scene_module_manager_interface.hpp | 8 +- .../scene_module_interface.hpp | 298 ------------------ .../src/behavior_path_planner_node.cpp | 8 - .../src/behavior_tree_manager.cpp | 4 +- .../src/scene_module/scene_module_visitor.cpp | 4 - 10 files changed, 126 insertions(+), 366 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp rename planning/behavior_path_planner/include/behavior_path_planner/{scene_module_v2 => scene_module}/scene_module_manager_interface.hpp (94%) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.hpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f290e8b8a3dab..43c85d7d592f0 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -15,6 +15,8 @@ set(common_src src/utilities.cpp src/path_utilities.cpp src/steering_factor_interface.cpp + src/behavior_path_planner_node.cpp + src/scene_module/scene_module_visitor.cpp src/turn_signal_decider.cpp src/util/avoidance/util.cpp src/util/lane_change/util.cpp @@ -40,9 +42,7 @@ set(common_src if(USE_BT) ament_auto_add_library(behavior_path_planner_node SHARED - src/behavior_path_planner_node.cpp src/behavior_tree_manager.cpp - src/scene_module/scene_module_visitor.cpp src/scene_module/scene_module_bt_node_interface.cpp src/scene_module/side_shift/side_shift_module.cpp src/scene_module/avoidance/avoidance_module.cpp @@ -60,12 +60,7 @@ if(USE_BT) else() ament_auto_add_library(behavior_path_planner_node SHARED - src/behavior_path_planner_node.cpp src/planner_manager.cpp - src/scene_module/scene_module_visitor.cpp - # src/scene_module_v2/avoidance - # src/scene_module_v2/lane_change - # src/scene_module_v2/... ${common_src} ) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index de3858560be98..ba45f4da7ce23 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" #ifdef USE_BEHAVIOR_TREE #include "behavior_path_planner/behavior_tree_manager.hpp" @@ -28,7 +29,6 @@ #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #else #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" #endif #include "behavior_path_planner/steering_factor_interface.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp b/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp new file mode 100644 index 0000000000000..87767ba2465a2 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ +#define BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ + +#ifdef USE_BEHAVIOR_TREE +#include +#endif + +namespace behavior_path_planner +{ + +#ifdef USE_BEHAVIOR_TREE +using ModuleStatus = BT::NodeStatus; +#else +enum class ModuleStatus { + IDLE = 0, + RUNNING = 1, + SUCCESS = 2, + FAILURE = 3, + // SKIPPED = 4, +}; +#endif + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 517c9fd355048..1baa547e27f07 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ #define BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ -#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 62f188277003e..bfe88d644f02c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/module_status.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utilities.hpp" @@ -30,8 +31,6 @@ #include #include -#include - #include #include #include @@ -59,16 +58,19 @@ class SceneModuleInterface : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, - uuid_(generateUUID()), is_waiting_approval_{false}, - current_state_{BT::NodeStatus::SUCCESS} + is_locked_new_module_launch_{false}, + uuid_(generateUUID()), + current_state_{ModuleStatus::SUCCESS} { +#ifdef USE_BEHAVIOR_TREE std::string module_ns; module_ns.resize(name.size()); std::transform(name.begin(), name.end(), module_ns.begin(), tolower); const auto ns = std::string("~/debug/") + module_ns; pub_debug_marker_ = node.create_publisher(ns, 20); +#endif } virtual ~SceneModuleInterface() = default; @@ -78,16 +80,13 @@ class SceneModuleInterface * FAILURE if plan has failed, RUNNING if plan is on going. * These condition is to be implemented in each modules. */ - virtual BT::NodeStatus updateState() = 0; + virtual ModuleStatus updateState() = 0; /** * @brief If the module plan customized reference path while waiting approval, it should output * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. */ - virtual BT::NodeStatus getNodeStatusWhileWaitingApproval() const - { - return BT::NodeStatus::FAILURE; - } + virtual ModuleStatus getNodeStatusWhileWaitingApproval() const { return ModuleStatus::FAILURE; } /** * @brief Return true if the module has request for execution (not necessarily feasible) @@ -135,7 +134,7 @@ class SceneModuleInterface */ virtual BehaviorModuleOutput run() { - current_state_ = BT::NodeStatus::RUNNING; + current_state_ = ModuleStatus::RUNNING; updateData(); @@ -196,25 +195,6 @@ class SceneModuleInterface steering_factor_interface_ptr_->publishSteeringFactor(clock_->now()); } - /** - * @brief set planner data - */ - void setData(const std::shared_ptr & data) { planner_data_ = data; } - - void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } - - std::string name() const { return name_; } - - rclcpp::Logger getLogger() const { return logger_; } - - std::shared_ptr planner_data_; - - bool isWaitingApproval() const { return is_waiting_approval_; } - - PlanResult getPathCandidate() const { return path_candidate_; } - - void resetPathCandidate() { path_candidate_.reset(); } - virtual void lockRTCCommand() { if (!rtc_interface_ptr_) { @@ -230,23 +210,56 @@ class SceneModuleInterface } rtc_interface_ptr_->unlockCommandUpdate(); } + + /** + * @brief set previous module's output as input for this module + */ + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + + /** + * @brief set planner data + */ + void setData(const std::shared_ptr & data) { planner_data_ = data; } + +#ifdef USE_BEHAVIOR_TREE + void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } +#endif + + bool isWaitingApproval() const { return is_waiting_approval_; } + + bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } + + void resetPathCandidate() { path_candidate_.reset(); } + + void resetPathReference() { path_reference_.reset(); } + + PlanResult getPathCandidate() const { return path_candidate_; } + + PlanResult getPathReference() const { return path_reference_; } + + MarkerArray getDebugMarkers() { return debug_marker_; } + + ModuleStatus getCurrentStatus() const { return current_state_; } + virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; + std::string name() const { return name_; } + + rclcpp::Logger getLogger() const { return logger_; } + private: std::string name_; + rclcpp::Logger logger_; -protected: - rclcpp::Clock::SharedPtr clock_; +#ifdef USE_BEHAVIOR_TREE rclcpp::Publisher::SharedPtr pub_debug_marker_; - mutable MarkerArray debug_marker_; - - std::shared_ptr rtc_interface_ptr_; - std::unique_ptr steering_factor_interface_ptr_; - UUID uuid_; - bool is_waiting_approval_; - PlanResult path_candidate_; +#endif +protected: void updateRTCStatus(const double start_distance, const double finish_distance) { if (!rtc_interface_ptr_) { @@ -264,12 +277,35 @@ class SceneModuleInterface rtc_interface_ptr_->clearCooperateStatus(); } + void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } + + void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; } + void waitApproval() { is_waiting_approval_ = true; } void clearWaitingApproval() { is_waiting_approval_ = false; } -public: - BT::NodeStatus current_state_; + rclcpp::Clock::SharedPtr clock_; + + std::shared_ptr rtc_interface_ptr_; + + std::shared_ptr planner_data_; + + std::unique_ptr steering_factor_interface_ptr_; + + bool is_waiting_approval_; + bool is_locked_new_module_launch_; + + UUID uuid_; + + PlanResult path_candidate_; + PlanResult path_reference_; + + ModuleStatus current_state_; + + BehaviorModuleOutput previous_module_output_; + + mutable MarkerArray debug_marker_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp similarity index 94% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index cdc0e0b0a3405..56af2cb2acd7e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include @@ -187,4 +187,4 @@ class SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.hpp deleted file mode 100644 index 3a5b04ed0af97..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module_v2/scene_module_interface.hpp +++ /dev/null @@ -1,298 +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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_INTERFACE_HPP_ - -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utilities.hpp" - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using rtc_interface::RTCInterface; -using steering_factor_interface::SteeringFactorInterface; -using tier4_autoware_utils::generateUUID; -using unique_identifier_msgs::msg::UUID; -using visualization_msgs::msg::MarkerArray; -using PlanResult = PathWithLaneId::SharedPtr; - -enum class ModuleStatus { - IDLE = 0, - RUNNING = 1, - SUCCESS = 2, - FAILURE = 3, - // SKIPPED = 4, -}; - -class SceneModuleInterface -{ -public: - SceneModuleInterface(const std::string & name, rclcpp::Node & node) - : name_{name}, - logger_{node.get_logger().get_child(name)}, - clock_{node.get_clock()}, - is_waiting_approval_{false}, - is_locked_new_module_launch_{false}, - uuid_(generateUUID()), - current_state_{ModuleStatus::SUCCESS} - { - } - - virtual ~SceneModuleInterface() = default; - - /** - * @brief Return SUCCESS if plan is not needed or plan is successfully finished, - * FAILURE if plan has failed, RUNNING if plan is on going. - * These condition is to be implemented in each modules. - */ - virtual ModuleStatus updateState() = 0; - - /** - * @brief If the module plan customized reference path while waiting approval, it should output - * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. - */ - virtual ModuleStatus getNodeStatusWhileWaitingApproval() const { return ModuleStatus::FAILURE; } - - /** - * @brief Return true if the module has request for execution (not necessarily feasible) - */ - virtual bool isExecutionRequested() const = 0; - - /** - * @brief Return true if the execution is available (e.g. safety check for lane change) - */ - virtual bool isExecutionReady() const = 0; - - /** - * @brief Calculate path. This function is called with the plan is approved. - */ - virtual BehaviorModuleOutput plan() = 0; - - /** - * @brief Calculate path under waiting_approval condition. - * The default implementation is just to return the reference path. - */ - virtual BehaviorModuleOutput planWaitingApproval() - { - BehaviorModuleOutput out; - out.path = util::generateCenterLinePath(planner_data_); - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - path_reference_ = previous_module_output_.reference_path; - return out; - } - - /** - * @brief Get candidate path. This information is used for external judgement. - */ - virtual CandidateOutput planCandidate() const = 0; - - /** - * @brief update data for planning. Note that the call of this function does not mean - * that the module executed. It should only updates the data necessary for - * planCandidate (e.g., resampling of path). - */ - virtual void updateData() {} - - /** - * @brief Execute module. Once this function is executed, - * it will continue to run as long as it is in the RUNNING state. - */ - virtual BehaviorModuleOutput run() - { - current_state_ = ModuleStatus::RUNNING; - - if (!isWaitingApproval()) { - return plan(); - } - - // module is waiting approval. Check it. - if (isActivated()) { - RCLCPP_DEBUG(logger_, "Was waiting approval, and now approved. Do plan()."); - return plan(); - } else { - RCLCPP_DEBUG(logger_, "keep waiting approval... Do planCandidate()."); - return planWaitingApproval(); - } - } - - /** - * @brief Called on the first time when the module goes into RUNNING. - */ - virtual void onEntry() = 0; - - /** - * @brief Called when the module exit from RUNNING. - */ - virtual void onExit() = 0; - - /** - * @brief Publish status if the module is requested to run - */ - virtual void publishRTCStatus() - { - if (!rtc_interface_ptr_) { - return; - } - rtc_interface_ptr_->publishCooperateStatus(clock_->now()); - } - - /** - * @brief Return true if the activation command is received - */ - virtual bool isActivated() - { - if (!rtc_interface_ptr_) { - return true; - } - if (rtc_interface_ptr_->isRegistered(uuid_)) { - return rtc_interface_ptr_->isActivated(uuid_); - } - return false; - } - - virtual void publishSteeringFactor() - { - if (!steering_factor_interface_ptr_) { - return; - } - steering_factor_interface_ptr_->publishSteeringFactor(clock_->now()); - } - - virtual void lockRTCCommand() - { - if (!rtc_interface_ptr_) { - return; - } - rtc_interface_ptr_->lockCommandUpdate(); - } - - virtual void unlockRTCCommand() - { - if (!rtc_interface_ptr_) { - return; - } - rtc_interface_ptr_->unlockCommandUpdate(); - } - - /** - * @brief set previous module's output as input for this module - */ - void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) - { - previous_module_output_ = previous_module_output; - } - - /** - * @brief set planner data - */ - void setData(const std::shared_ptr & data) { planner_data_ = data; } - - void resetPathCandidate() { path_candidate_.reset(); } - - void resetPathReference() { path_reference_.reset(); } - - bool isWaitingApproval() const { return is_waiting_approval_; } - - bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } - - rclcpp::Logger getLogger() const { return logger_; } - - PlanResult getPathCandidate() const { return path_candidate_; } - - PlanResult getPathReference() const { return path_reference_; } - - MarkerArray getDebugMarkers() { return debug_marker_; } - - ModuleStatus getCurrentStatus() const { return current_state_; } - - virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; - - std::string name() const { return name_; } - - std::shared_ptr planner_data_; - -private: - std::string name_; - rclcpp::Logger logger_; - -protected: - void updateRTCStatus(const double start_distance, const double finish_distance) - { - if (!rtc_interface_ptr_) { - return; - } - rtc_interface_ptr_->updateCooperateStatus( - uuid_, isExecutionReady(), start_distance, finish_distance, clock_->now()); - } - - virtual void removeRTCStatus() - { - if (!rtc_interface_ptr_) { - return; - } - rtc_interface_ptr_->clearCooperateStatus(); - } - - void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } - - void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; } - - void waitApproval() { is_waiting_approval_ = true; } - - void clearWaitingApproval() { is_waiting_approval_ = false; } - - rclcpp::Clock::SharedPtr clock_; - - std::shared_ptr rtc_interface_ptr_; - std::unique_ptr steering_factor_interface_ptr_; - - bool is_waiting_approval_; - bool is_locked_new_module_launch_; - - UUID uuid_; - - PlanResult path_candidate_; - PlanResult path_reference_; - - ModuleStatus current_state_; - - BehaviorModuleOutput previous_module_output_; - - mutable MarkerArray debug_marker_; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE_V2__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 601542fd34d43..51bb5dab138fc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1117,11 +1117,7 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( { const auto target_module = "PullOver"; -#ifdef USE_BEHAVIOR_TREE - const auto target_status = BT::NodeStatus::RUNNING; -#else const auto target_status = ModuleStatus::RUNNING; -#endif for (auto & status : statuses) { if (status->is_waiting_approval || status->status == target_status) { @@ -1139,11 +1135,7 @@ bool BehaviorPathPlannerNode::keepInputPoints( { const std::vector target_modules = {"PullOver", "Avoidance"}; -#ifdef USE_BEHAVIOR_TREE - const auto target_status = BT::NodeStatus::RUNNING; -#else const auto target_status = ModuleStatus::RUNNING; -#endif for (auto & status : statuses) { if (status->is_waiting_approval || status->status == target_status) { diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index 1c441426e8dea..0f016c8e5836b 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -121,10 +121,10 @@ void BehaviorTreeManager::resetNotRunningModulePathCandidate() { const bool is_any_module_running = std::any_of( scene_modules_.begin(), scene_modules_.end(), - [](const auto & module) { return module->current_state_ == BT::NodeStatus::RUNNING; }); + [](const auto & module) { return module->getCurrentStatus() == BT::NodeStatus::RUNNING; }); for (auto & module : scene_modules_) { - if (is_any_module_running && (module->current_state_ != BT::NodeStatus::RUNNING)) { + if (is_any_module_running && (module->getCurrentStatus() != BT::NodeStatus::RUNNING)) { module->resetPathCandidate(); } } diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp index c622fdc0532ea..42c8ae15919b1 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp +++ b/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp @@ -14,11 +14,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#ifdef USE_BEHAVIOR_TREE #include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#else -#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp" -#endif namespace behavior_path_planner { From f9075cbcfe88fc0dc81f13e3f9b72e5ff38f73c5 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 2 Mar 2023 10:46:42 +0900 Subject: [PATCH 11/24] feat(avoidance): enable to avoid vehicles that stopped for a long time (#2938) * feat(utilites): add getDistanceToNextTrafficLight() Signed-off-by: satoshi-ota * feat(avoidance): add new params for force avoidance Signed-off-by: satoshi-ota * feat(avoidance): avoid all vehicle that stopped long duration Signed-off-by: satoshi-ota * feat(avoidance): output stop time & stop factor distance Signed-off-by: satoshi-ota * feat(avoidance): add new param Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 3 + .../util/avoidance/avoidance_module_data.hpp | 16 +++++ .../behavior_path_planner/utilities.hpp | 3 + .../src/behavior_path_planner_node.cpp | 6 ++ .../src/marker_util/avoidance/debug.cpp | 6 +- .../avoidance/avoidance_module.cpp | 63 ++++++++++++++----- .../behavior_path_planner/src/utilities.cpp | 25 ++++++++ 7 files changed, 104 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index e42f188a62445..c739d0eb73774 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -13,6 +13,7 @@ enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false + enable_force_avoidance_for_stopped_vehicle: false enable_safety_check: false enable_yield_maneuver: false @@ -36,7 +37,9 @@ # filtering moving objects threshold_speed_object_is_stopped: 1.0 # [m/s] threshold_time_object_is_moving: 1.0 # [s] + threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] # detection range + object_check_force_avoidance_clearance: 30.0 # [m] object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] object_check_goal_distance: 20.0 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp index 8b81a3e3d673c..8f44fdc3fedbf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp @@ -70,6 +70,9 @@ struct AvoidanceParameters // enable update path when if detected objects on planner data is gone. bool enable_update_path_when_object_is_gone{false}; + // enable avoidance for all parking vehicle + bool enable_force_avoidance_for_stopped_vehicle{false}; + // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver bool enable_safety_check{false}; @@ -101,6 +104,9 @@ struct AvoidanceParameters // vehicles with speed greater than this will not be avoided double threshold_speed_object_is_stopped; + // execute only when there is no intersection or crosswalk behind of the stopped vehicle. + double object_check_force_avoidance_clearance; + // distance to avoid object detection double object_check_forward_distance; @@ -123,6 +129,9 @@ struct AvoidanceParameters // vehicles which is moving more than this parameter will not be avoided double threshold_time_object_is_moving; + // force avoidance + double threshold_time_force_avoidance_for_stopped_vehicle; + // we want to keep this lateral margin when avoiding double lateral_collision_margin; // a buffer in case lateral_collision_margin is set to 0. Will throw error @@ -290,6 +299,10 @@ struct ObjectData // avoidance target rclcpp::Time last_stop; double move_time{0.0}; + // object stopping duration + rclcpp::Time last_move; + double stop_time{0.0}; + // store the information of the lanelet which the object's overhang is currently occupying lanelet::ConstLanelet overhang_lanelet; @@ -305,6 +318,9 @@ struct ObjectData // avoidance target // lateral distance from overhang to the road shoulder double to_road_shoulder_distance{0.0}; + // to intersection + double to_stop_factor_distance{std::numeric_limits::max()}; + // if lateral margin is NOT enough, the ego must avoid the object. bool avoid_required{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 91058b2cccdb3..89f1beedefd6d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -140,6 +140,9 @@ double l2Norm(const Vector3 vector); double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets); +double getDistanceToNextTrafficLight( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets); + double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 51bb5dab138fc..69a29401d937e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -357,6 +357,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "enable_avoidance_over_opposite_direction"); p.enable_update_path_when_object_is_gone = declare_parameter(ns + "enable_update_path_when_object_is_gone"); + p.enable_force_avoidance_for_stopped_vehicle = + declare_parameter(ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); p.disable_path_update = declare_parameter(ns + "disable_path_update"); @@ -384,6 +386,10 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "threshold_speed_object_is_stopped"); p.threshold_time_object_is_moving = declare_parameter(ns + "threshold_time_object_is_moving"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + declare_parameter(ns + "threshold_time_force_avoidance_for_stopped_vehicle"); + p.object_check_force_avoidance_clearance = + declare_parameter(ns + "object_check_force_avoidance_clearance"); p.object_check_forward_distance = declare_parameter(ns + "object_check_forward_distance"); p.object_check_backward_distance = diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index 8b6898b20b7ab..af0278a5d3f9d 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -79,12 +79,16 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st for (const auto & object : objects) { { + const auto to_stop_factor_distance = std::min(object.to_stop_factor_distance, 1000.0); marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2); string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral: " << object.lateral << " [-]"; + << "lateral: " << object.lateral << " [-]\n" + << "stop_factor:" << to_stop_factor_distance << " [m]\n" + << "move_time:" << object.move_time << " [s]\n" + << "stop_time:" << object.stop_time << " [s]\n"; marker.text = string_stream.str(); msg.markers.push_back(marker); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 48bc7af2fbe71..527bb50c5dabf 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -405,6 +405,44 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } } + // calculate avoid_margin dynamically + // NOTE: This calculation must be after calculating to_road_shoulder_distance. + const double max_avoid_margin = parameters_->lateral_collision_safety_buffer + + parameters_->lateral_collision_margin + 0.5 * vehicle_width; + const double min_safety_lateral_distance = + parameters_->lateral_collision_safety_buffer + 0.5 * vehicle_width; + const auto max_allowable_lateral_distance = object_data.to_road_shoulder_distance - + parameters_->road_shoulder_safety_margin - + 0.5 * vehicle_width; + + const auto avoid_margin = [&]() -> boost::optional { + if (max_allowable_lateral_distance < min_safety_lateral_distance) { + return boost::none; + } + return std::min(max_allowable_lateral_distance, max_avoid_margin); + }(); + + // force avoidance for stopped vehicle + { + const auto to_traffic_light = + util::getDistanceToNextTrafficLight(object_pose, data.current_lanelets); + + object_data.to_stop_factor_distance = + std::min(to_traffic_light, object_data.to_stop_factor_distance); + } + + if ( + object_data.stop_time > parameters_->threshold_time_force_avoidance_for_stopped_vehicle && + parameters_->enable_force_avoidance_for_stopped_vehicle) { + if ( + object_data.to_stop_factor_distance > parameters_->object_check_force_avoidance_clearance) { + object_data.last_seen = clock_->now(); + object_data.avoid_margin = avoid_margin; + data.target_objects.push_back(object_data); + continue; + } + } + DEBUG_PRINT( "set object_data: longitudinal = %f, lateral = %f, largest_overhang = %f," "to_road_shoulder_distance = %f", @@ -506,23 +544,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } object_data.last_seen = clock_->now(); - - // calculate avoid_margin dynamically - // NOTE: This calculation must be after calculating to_road_shoulder_distance. - const double max_avoid_margin = parameters_->lateral_collision_safety_buffer + - parameters_->lateral_collision_margin + 0.5 * vehicle_width; - const double min_safety_lateral_distance = - parameters_->lateral_collision_safety_buffer + 0.5 * vehicle_width; - const auto max_allowable_lateral_distance = object_data.to_road_shoulder_distance - - parameters_->road_shoulder_safety_margin - - 0.5 * vehicle_width; - - object_data.avoid_margin = [&]() -> boost::optional { - if (max_allowable_lateral_distance < min_safety_lateral_distance) { - return boost::none; - } - return std::min(max_allowable_lateral_distance, max_avoid_margin); - }(); + object_data.avoid_margin = avoid_margin; // set data data.target_objects.push_back(object_data); @@ -583,21 +605,28 @@ void AvoidanceModule::fillObjectMovingTime(ObjectData & object_data) const object_data.last_stop = clock_->now(); object_data.move_time = 0.0; if (is_new_object) { + object_data.stop_time = 0.0; + object_data.last_move = clock_->now(); stopped_objects_.push_back(object_data); } else { + same_id_obj->stop_time = (clock_->now() - same_id_obj->last_move).seconds(); same_id_obj->last_stop = clock_->now(); same_id_obj->move_time = 0.0; + object_data.stop_time = same_id_obj->stop_time; } return; } if (is_new_object) { object_data.move_time = std::numeric_limits::max(); + object_data.stop_time = 0.0; + object_data.last_move = clock_->now(); return; } object_data.last_stop = same_id_obj->last_stop; object_data.move_time = (clock_->now() - same_id_obj->last_stop).seconds(); + object_data.stop_time = 0.0; if (object_data.move_time > parameters_->threshold_time_object_is_moving) { stopped_objects_.erase(same_id_obj); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 04daf4053784e..d34ca2ab819ee 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1235,6 +1235,31 @@ double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLan return lanelet_length - arc_coordinates.length; } +double getDistanceToNextTrafficLight( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets) +{ + const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); + + lanelet::ConstLanelet current_lanelet; + if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { + return std::numeric_limits::max(); + } + + double distance = 0; + bool is_after_current_lanelet = false; + for (const auto & llt : lanelets) { + if (llt == current_lanelet) { + is_after_current_lanelet = true; + } + if (is_after_current_lanelet && !llt.regulatoryElementsAs().empty()) { + return distance - arc_coordinates.length; + } + distance += lanelet::utils::getLaneletLength3d(llt); + } + + return std::numeric_limits::max(); +} + double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { From 1a8099a5d277e8a731602fa6ba5ee025fa0ee01a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 2 Mar 2023 11:23:17 +0900 Subject: [PATCH 12/24] fix(behavior_path_planner): rename macro (#2978) Signed-off-by: satoshi-ota --- planning/behavior_path_planner/CMakeLists.txt | 12 ++--- .../behavior_path_planner_node.hpp | 16 +++---- .../behavior_path_planner/module_status.hpp | 4 +- .../scene_module/scene_module_interface.hpp | 6 +-- .../src/behavior_path_planner_node.cpp | 46 +++++++++---------- 5 files changed, 41 insertions(+), 43 deletions(-) diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 43c85d7d592f0..a636b1ea0d766 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,9 +7,7 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -# use planner manager that supports behavior tree -set(USE_BT TRUE) -message(STATUS "USE_BEHAVIOR_TREE: " ${USE_BT}) +set(COMPILE_WITH_OLD_ARCHITECTURE TRUE) set(common_src src/utilities.cpp @@ -40,7 +38,7 @@ set(common_src src/marker_util/lane_change/debug.cpp ) -if(USE_BT) +if(COMPILE_WITH_OLD_ARCHITECTURE) ament_auto_add_library(behavior_path_planner_node SHARED src/behavior_tree_manager.cpp src/scene_module/scene_module_bt_node_interface.cpp @@ -54,9 +52,9 @@ if(USE_BT) ${common_src} ) - target_compile_definitions(behavior_path_planner_node PRIVATE USE_BEHAVIOR_TREE) + target_compile_definitions(behavior_path_planner_node PRIVATE USE_OLD_ARCHITECTURE) - message(WARNING "Build behavior_path_planner with BT...") + message(WARNING "Build behavior_path_planner with OLD framework...") else() ament_auto_add_library(behavior_path_planner_node SHARED @@ -64,7 +62,7 @@ else() ${common_src} ) - message(WARNING "Build behavior_path_planner without BT...") + message(WARNING "Build behavior_path_planner with NEW framework...") endif() target_include_directories(behavior_path_planner_node SYSTEM PUBLIC diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index ba45f4da7ce23..959b7579e65cb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -18,7 +18,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp" @@ -107,7 +107,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE rclcpp::Subscription::SharedPtr operation_mode_subscriber_; #endif rclcpp::Publisher::SharedPtr path_publisher_; @@ -118,13 +118,13 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE std::map::SharedPtr> path_reference_publishers_; #endif std::shared_ptr planner_data_; -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE std::shared_ptr bt_manager_; #else std::shared_ptr planner_manager_; @@ -149,7 +149,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node // update planner data std::shared_ptr createLatestPlannerData(); -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE // parameters std::shared_ptr avoidance_param_ptr; std::shared_ptr lane_change_param_ptr; @@ -157,7 +157,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node BehaviorPathPlannerParameters getCommonParam(); -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE BehaviorTreeManagerParam getBehaviorTreeManagerParam(); SideShiftParameters getSideShiftParam(); AvoidanceParameters getAvoidanceParam(); @@ -174,7 +174,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE void onOperationMode(const OperationModeState::ConstSharedPtr msg); #endif SetParametersResult onSetParam(const std::vector & parameters); @@ -235,7 +235,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish path candidate */ -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE void publishPathCandidate( const std::vector> & scene_modules); #else diff --git a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp b/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp index 87767ba2465a2..cfb5209ff795c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp @@ -15,14 +15,14 @@ #ifndef BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ #define BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE #include #endif namespace behavior_path_planner { -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE using ModuleStatus = BT::NodeStatus; #else enum class ModuleStatus { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index bfe88d644f02c..28bac155adb98 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -63,7 +63,7 @@ class SceneModuleInterface uuid_(generateUUID()), current_state_{ModuleStatus::SUCCESS} { -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE std::string module_ns; module_ns.resize(name.size()); std::transform(name.begin(), name.end(), module_ns.begin(), tolower); @@ -224,7 +224,7 @@ class SceneModuleInterface */ void setData(const std::shared_ptr & data) { planner_data_ = data; } -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } #endif @@ -255,7 +255,7 @@ class SceneModuleInterface rclcpp::Logger logger_; -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE rclcpp::Publisher::SharedPtr pub_debug_marker_; #endif diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 69a29401d937e..38a41743c1113 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -91,7 +91,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod occupancy_grid_subscriber_ = create_subscription( "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE operation_mode_subscriber_ = create_subscription( "/system/operation_mode/state", 1, std::bind(&BehaviorPathPlannerNode::onOperationMode, this, _1), @@ -113,7 +113,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1), createSubscriptionOptions(this)); -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE avoidance_param_ptr = std::make_shared(getAvoidanceParam()); lane_change_param_ptr = std::make_shared(getLaneChangeParam()); #endif @@ -121,7 +121,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod m_set_param_res = this->add_on_set_parameters_callback( std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE // behavior tree manager { RCLCPP_INFO(get_logger(), "use behavior tree."); @@ -224,7 +224,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE p.verbose = declare_parameter("verbose"); #endif @@ -305,7 +305,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() return p; } -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -329,7 +329,7 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() } #endif -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() { AvoidanceParameters p{}; @@ -491,7 +491,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() } #endif -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() { LaneFollowingParameters p{}; @@ -507,7 +507,7 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() } #endif -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -586,7 +586,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() } #endif -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE PullOverParameters BehaviorPathPlannerNode::getPullOverParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -684,7 +684,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() } #endif -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE PullOutParameters BehaviorPathPlannerNode::getPullOutParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -742,7 +742,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() } #endif -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() { BehaviorTreeManagerParam p{}; @@ -785,7 +785,7 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("self_acceleration"); } -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE if (!planner_data_->operation_mode) { return missing("operation_mode"); } @@ -812,7 +812,7 @@ std::shared_ptr BehaviorPathPlannerNode::createLatestPlannerData() // so that the each modules do not have to care about the "route jump". if (!is_first_time) { RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE bt_manager_->resetBehaviorTree(); #else planner_manager_->reset(); @@ -843,14 +843,14 @@ void BehaviorPathPlannerNode::run() // create latest planner data const auto planner_data = createLatestPlannerData(); -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE if (planner_data->operation_mode->mode != OperationModeState::AUTONOMOUS) { planner_manager_->resetRootLanelet(planner_data); } #endif // run behavior planner -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE const auto output = bt_manager_->run(planner_data); #else const auto output = planner_manager_->run(planner_data); @@ -878,7 +878,7 @@ void BehaviorPathPlannerNode::run() get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); } -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE publishPathCandidate(bt_manager_->getSceneModules()); #else publishPathCandidate(planner_manager_->getSceneModuleManagers()); @@ -899,7 +899,7 @@ void BehaviorPathPlannerNode::run() debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); } -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE planner_manager_->print(); planner_manager_->publishDebugMarker(); #endif @@ -994,7 +994,7 @@ void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) void BehaviorPathPlannerNode::publishSceneModuleDebugMsg() { -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE const auto debug_messages_data_ptr = bt_manager_->getAllSceneModuleDebugMsgData(); const auto avoidance_debug_message = debug_messages_data_ptr->getAvoidanceModuleDebugMsg(); @@ -1009,7 +1009,7 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg() #endif } -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE void BehaviorPathPlannerNode::publishPathCandidate( const std::vector> & scene_modules) { @@ -1101,7 +1101,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); PathWithLaneId connected_path; -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE const auto module_status_ptr_vec = bt_manager_->getModulesStatus(); #else const auto module_status_ptr_vec = planner_manager_->getSceneModuleStatus(); @@ -1187,7 +1187,7 @@ void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) route_ptr_ = msg; has_received_route_ = true; } -#ifndef USE_BEHAVIOR_TREE +#ifndef USE_OLD_ARCHITECTURE void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSharedPtr msg) { const std::lock_guard lock(mutex_pd_); @@ -1200,7 +1200,7 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( { rcl_interfaces::msg::SetParametersResult result; -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE if (!lane_change_param_ptr && !avoidance_param_ptr) { result.successful = false; result.reason = "param not initialized"; @@ -1212,7 +1212,7 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( result.reason = "success"; try { -#ifdef USE_BEHAVIOR_TREE +#ifdef USE_OLD_ARCHITECTURE update_param( parameters, "avoidance.publish_debug_marker", avoidance_param_ptr->publish_debug_marker); update_param( From ede21f3b7fa2fe79470c0740d902aea9d3d5949e Mon Sep 17 00:00:00 2001 From: ryohei sasaki Date: Thu, 2 Mar 2023 02:49:14 -0500 Subject: [PATCH 13/24] feat(pose_initilizer): support gnss/imu pose estimator (#2904) * Support GNSS/IMU pose estimator Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Revert gnss/imu support Signed-off-by: Ryohei Sasaki * Support GNSS/IMU pose estimator Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Separate EKF and NDT trigger modules Signed-off-by: Ryohei Sasaki * Integrate activate and deactivate into sendRequest Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Change sendRequest function arguments Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Remove unused conditional branches Signed-off-by: Ryohei Sasaki * Fix command name Signed-off-by: Ryohei Sasaki * Change to snake_case Signed-off-by: Ryohei Sasaki * Fix typos Signed-off-by: Ryohei Sasaki * Update localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Signed-off-by: Ryohei Sasaki * Update localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Signed-off-by: Ryohei Sasaki * Update copyright year Signed-off-by: Ryohei Sasaki * Set the copyright year of ekf_localization_module to 2022 Signed-off-by: Ryohei Sasaki * Delete unnecessary conditional branches Signed-off-by: Ryohei Sasaki * Add ekf_enabled parameter Signed-off-by: Ryohei Sasaki * Add #include Signed-off-by: Ryohei Sasaki --------- Signed-off-by: Ryohei Sasaki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Ryohei Sasaki Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- .../launch/util/util.launch.xml | 1 + .../launch/simulator.launch.xml | 1 + localization/pose_initializer/CMakeLists.txt | 3 +- localization/pose_initializer/README.md | 1 + .../launch/pose_initializer.launch.xml | 2 + .../ekf_localization_trigger_module.cpp | 56 ++++++++++++++ ...pp => ekf_localization_trigger_module.hpp} | 14 ++-- .../localization_trigger_module.cpp | 75 ------------------- .../ndt_localization_trigger_module.cpp | 56 ++++++++++++++ .../ndt_localization_trigger_module.hpp | 36 +++++++++ .../pose_initializer_core.cpp | 22 ++++-- .../pose_initializer_core.hpp | 6 +- 12 files changed, 181 insertions(+), 92 deletions(-) create mode 100644 localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp rename localization/pose_initializer/src/pose_initializer/{localization_trigger_module.hpp => ekf_localization_trigger_module.hpp} (68%) delete mode 100644 localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp create mode 100644 localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp create mode 100644 localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index 1dfe2e3d5f109..d820e06deb614 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -16,6 +16,7 @@ + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index d48485bc3ff06..aa1877be4f536 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -99,6 +99,7 @@ + diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index eca7018db8e57..d048cd1cce26f 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/localization/pose_initializer/CMakeLists.txt @@ -10,7 +10,8 @@ ament_auto_add_executable(pose_initializer_node src/pose_initializer/gnss_module.cpp src/pose_initializer/ndt_module.cpp src/pose_initializer/stop_check_module.cpp - src/pose_initializer/localization_trigger_module.cpp + src/pose_initializer/ekf_localization_trigger_module.cpp + src/pose_initializer/ndt_localization_trigger_module.cpp ) if(BUILD_TESTING) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 9b13fda32fa93..81cfe3c4a0665 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -15,6 +15,7 @@ This node depends on the map height fitter library. | Name | Type | Description | | --------------------- | ---- | ---------------------------------------------------------------------------------------- | +| `ekf_enabled` | bool | If true, EKF localizar is activated. | | `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | | `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | | `stop_check_duration` | bool | The duration used for the stop check above. | diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 7d73cf26b4766..9483ff47bc093 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -2,10 +2,12 @@ + + diff --git a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp new file mode 100644 index 0000000000000..ac9796b687637 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp @@ -0,0 +1,56 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 "ekf_localization_trigger_module.hpp" + +#include +#include + +#include +#include + +using ServiceException = component_interface_utils::ServiceException; +using Initialize = localization_interface::Initialize; + +EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) +: logger_(node->get_logger()) +{ + client_ekf_trigger_ = node->create_client("ekf_trigger_node"); +} + +void EkfLocalizationTriggerModule::send_request(bool flag) const +{ + const auto req = std::make_shared(); + std::string command_name; + req->data = flag; + if (flag) { + command_name = "Activation"; + } else { + command_name = "Deactivation"; + } + + if (!client_ekf_trigger_->service_is_ready()) { + throw component_interface_utils::ServiceUnready("EKF triggering service is not ready"); + } + + auto future_ekf = client_ekf_trigger_->async_send_request(req); + + if (future_ekf.get()->success) { + RCLCPP_INFO(logger_, "EKF %s succeeded", command_name.c_str()); + } else { + RCLCPP_INFO(logger_, "EKF %s failed", command_name.c_str()); + throw ServiceException( + Initialize::Service::Response::ERROR_ESTIMATION, "EKF " + command_name + " failed"); + } +} diff --git a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp similarity index 68% rename from localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp rename to localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp index 3e32518c89698..d1b8eb986105f 100644 --- a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp @@ -12,27 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_ -#define POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_ +#ifndef POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_ +#define POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_ #include #include -class LocalizationTriggerModule +class EkfLocalizationTriggerModule { private: using SetBool = std_srvs::srv::SetBool; public: - explicit LocalizationTriggerModule(rclcpp::Node * node); - void deactivate() const; - void activate() const; + explicit EkfLocalizationTriggerModule(rclcpp::Node * node); + void send_request(bool flag) const; private: rclcpp::Logger logger_; rclcpp::Client::SharedPtr client_ekf_trigger_; - rclcpp::Client::SharedPtr client_ndt_trigger_; }; -#endif // POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_ +#endif // POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_ diff --git a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp deleted file mode 100644 index b197eea3f1d9a..0000000000000 --- a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// 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.S -#include "localization_trigger_module.hpp" - -#include -#include - -#include - -using ServiceException = component_interface_utils::ServiceException; -using Initialize = localization_interface::Initialize; - -LocalizationTriggerModule::LocalizationTriggerModule(rclcpp::Node * node) -: logger_(node->get_logger()) -{ - client_ekf_trigger_ = node->create_client("ekf_trigger_node"); - client_ndt_trigger_ = node->create_client("ndt_trigger_node"); -} - -void LocalizationTriggerModule::deactivate() const -{ - const auto req = std::make_shared(); - req->data = false; - - if (!client_ekf_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("EKF triggering service is not ready"); - } - if (!client_ndt_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("NDT triggering service is not ready"); - } - - auto future_ekf = client_ekf_trigger_->async_send_request(req); - auto future_ndt = client_ndt_trigger_->async_send_request(req); - - if (future_ekf.get()->success & future_ndt.get()->success) { - RCLCPP_INFO(logger_, "Deactivation succeeded"); - } else { - RCLCPP_INFO(logger_, "Deactivation failed"); - throw ServiceException(Initialize::Service::Response::ERROR_ESTIMATION, "Deactivation failed"); - } -} - -void LocalizationTriggerModule::activate() const -{ - const auto req = std::make_shared(); - req->data = true; - - if (!client_ekf_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("EKF triggering service is not ready"); - } - if (!client_ndt_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("NDT triggering service is not ready"); - } - - auto future_ekf = client_ekf_trigger_->async_send_request(req); - auto future_ndt = client_ndt_trigger_->async_send_request(req); - - if (future_ekf.get()->success & future_ndt.get()->success) { - RCLCPP_INFO(logger_, "Activation succeeded"); - } else { - RCLCPP_INFO(logger_, "Activation failed"); - throw ServiceException(Initialize::Service::Response::ERROR_ESTIMATION, "Activation failed"); - } -} diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp new file mode 100644 index 0000000000000..e1285f5c31c83 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp @@ -0,0 +1,56 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "ndt_localization_trigger_module.hpp" + +#include +#include + +#include +#include + +using ServiceException = component_interface_utils::ServiceException; +using Initialize = localization_interface::Initialize; + +NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) +: logger_(node->get_logger()) +{ + client_ndt_trigger_ = node->create_client("ndt_trigger_node"); +} + +void NdtLocalizationTriggerModule::send_request(bool flag) const +{ + const auto req = std::make_shared(); + std::string command_name; + req->data = flag; + if (flag) { + command_name = "Activation"; + } else { + command_name = "Deactivation"; + } + + if (!client_ndt_trigger_->service_is_ready()) { + throw component_interface_utils::ServiceUnready("NDT triggering service is not ready"); + } + + auto future_ndt = client_ndt_trigger_->async_send_request(req); + + if (future_ndt.get()->success) { + RCLCPP_INFO(logger_, "NDT %s succeeded", command_name.c_str()); + } else { + RCLCPP_INFO(logger_, "NDT %s failed", command_name.c_str()); + throw ServiceException( + Initialize::Service::Response::ERROR_ESTIMATION, "NDT " + command_name + " failed"); + } +} diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp new file mode 100644 index 0000000000000..91e37c9bb90e9 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_ +#define POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_ + +#include + +#include + +class NdtLocalizationTriggerModule +{ +private: + using SetBool = std_srvs::srv::SetBool; + +public: + explicit NdtLocalizationTriggerModule(rclcpp::Node * node); + void send_request(bool flag) const; + +private: + rclcpp::Logger logger_; + rclcpp::Client::SharedPtr client_ndt_trigger_; +}; + +#endif // POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_ diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 7222ac2c4a244..e4f590a7b1f42 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -15,8 +15,9 @@ #include "pose_initializer_core.hpp" #include "copy_vector_to_array.hpp" +#include "ekf_localization_trigger_module.hpp" #include "gnss_module.hpp" -#include "localization_trigger_module.hpp" +#include "ndt_localization_trigger_module.hpp" #include "ndt_module.hpp" #include "stop_check_module.hpp" @@ -34,12 +35,15 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); + if (declare_parameter("ekf_enabled")) { + ekf_localization_trigger_ = std::make_unique(this); + } if (declare_parameter("gnss_enabled")) { gnss_ = std::make_unique(this); } if (declare_parameter("ndt_enabled")) { ndt_ = std::make_unique(this); - localization_trigger_ = std::make_unique(this); + ndt_localization_trigger_ = std::make_unique(this); } if (declare_parameter("stop_check_enabled")) { // Add 1.0 sec margin for twist buffer. @@ -73,8 +77,11 @@ void PoseInitializer::on_initialize( } try { change_state(State::Message::INITIALIZING); - if (localization_trigger_) { - localization_trigger_->deactivate(); + if (ekf_localization_trigger_) { + ekf_localization_trigger_->send_request(false); + } + if (ndt_localization_trigger_) { + ndt_localization_trigger_->send_request(false); } auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); if (ndt_) { @@ -82,8 +89,11 @@ void PoseInitializer::on_initialize( } pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); - if (localization_trigger_) { - localization_trigger_->activate(); + if (ekf_localization_trigger_) { + ekf_localization_trigger_->send_request(true); + } + if (ndt_localization_trigger_) { + ndt_localization_trigger_->send_request(true); } res->status.success = true; change_state(State::Message::INITIALIZED); diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index a80ed5ea954ea..5fb6677b6b71f 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -26,7 +26,8 @@ class StopCheckModule; class NdtModule; class GnssModule; -class LocalizationTriggerModule; +class EkfLocalizationTriggerModule; +class NdtLocalizationTriggerModule; class PoseInitializer : public rclcpp::Node { @@ -50,7 +51,8 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr gnss_; std::unique_ptr ndt_; std::unique_ptr stop_check_; - std::unique_ptr localization_trigger_; + std::unique_ptr ekf_localization_trigger_; + std::unique_ptr ndt_localization_trigger_; double stop_check_duration_; void change_state(State::Message::_state_type state); void on_initialize( From 261db5c8f5f87c16d29cf39f0336ba6b5590a637 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 2 Mar 2023 18:27:51 +0900 Subject: [PATCH 14/24] fix(tier4_state_rviz_plugin): fix typo (#2988) Signed-off-by: satoshi-ota --- common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 5a36f706f38d9..1cfe91e610abd 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -188,7 +188,7 @@ QGroupBox * AutowareStatePanel::makeMotionGroup() QGroupBox * AutowareStatePanel::makeFailSafeGroup() { - auto * group = new QGroupBox("FalSafe"); + auto * group = new QGroupBox("FailSafe"); auto * grid = new QGridLayout; mrm_state_label_ptr_ = new QLabel("INIT"); From 91d80137d46ea8d6092ce7385c8525ace032ee44 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 2 Mar 2023 20:24:16 +0900 Subject: [PATCH 15/24] fix(motion_velocity_smoother): fix terminal velocity optimization (#2989) * fix(motion_velocity_smoother): fix terminal velocity optimization Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../src/smoother/jerk_filtered_smoother.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 580d8e0c44f5a..04dc06cb0bb18 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -198,12 +198,15 @@ bool JerkFilteredSmoother::apply( P(IDX_A0 + i + 1, IDX_A0 + i + 1) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; } + // |v_max_i^2 - b_i|/v_max^2 -> minimize (-bi) * ds / v_max^2 for (size_t i = 0; i < N; ++i) { - const double v_max = std::max(v_max_arr.at(i), 0.1); - q.at(IDX_B0 + i) = - -1.0 / (v_max * v_max); // |v_max_i^2 - b_i|/v_max^2 -> minimize (-bi) * ds / v_max^2 - if (i < N - 1) { - q.at(IDX_B0 + i) *= std::max(interval_dist_arr.at(i), 0.0001); + if (v_max_arr.at(i) > 0.01) { + // Note that if v_max[i] is too small, we did not minimize the corresponding -b[i] + double v_weight_term = -1.0 / (v_max_arr.at(i) * v_max_arr.at(i)); + if (i < N - 1) { + v_weight_term *= std::max(interval_dist_arr.at(i), 0.0001); + } + q.at(IDX_B0 + i) += v_weight_term; } P(IDX_DELTA0 + i, IDX_DELTA0 + i) += over_v_weight; // over velocity cost P(IDX_SIGMA0 + i, IDX_SIGMA0 + i) += over_a_weight; // over acceleration cost From 8ce056f94d93403c7b6972abaa4219d927d3ea0f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 3 Mar 2023 09:55:18 +0900 Subject: [PATCH 16/24] chore(typo): eliminate typos (#2216) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Replace 'asssert' with 'assert' Signed-off-by: Kotaro Yoshimoto * fix(typo): computationall => computational Signed-off-by: Kotaro Yoshimoto * fix(typo): collinearity => collinearity Signed-off-by: Kotaro Yoshimoto * fix(typo): hypothenus => hypotenuse Signed-off-by: Kotaro Yoshimoto * fix(typo): numbef => number Signed-off-by: Kotaro Yoshimoto * fix(typo): missmatched => mismatched Signed-off-by: Kotaro Yoshimoto * fix(typo): minimun => minimum Signed-off-by: Kotaro Yoshimoto * fix(typo): neighbore => neighbor Signed-off-by: Kotaro Yoshimoto * fix(typo): neighbour => neighbor Signed-off-by: Kotaro Yoshimoto * fix(typo): propery => properly Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * fix(typo): reagion => region Signed-off-by: Kotaro Yoshimoto * fix(typo): shirinking => shrinking Signed-off-by: Kotaro Yoshimoto * fix(typo): turining => turning Signed-off-by: Kotaro Yoshimoto * fix(typo): lexas => lexus Signed-off-by: Kotaro Yoshimoto * fix(typo): fastetst => fastest Signed-off-by: Kotaro Yoshimoto * fix(typo): analyse => analyze Signed-off-by: Kotaro Yoshimoto * fix(typo): ordinaray => ordinary Signed-off-by: Kotaro Yoshimoto * fix(typo): existance => existence Signed-off-by: Kotaro Yoshimoto * fix(typo): insert missing space Signed-off-by: Kotaro Yoshimoto * fix(typo): modify url including typo in original url Signed-off-by: Kotaro Yoshimoto * fix(typo): precompined => precomputed Signed-off-by: Kotaro Yoshimoto * fix(typo): magitude => magnitude Signed-off-by: Kotaro Yoshimoto * fix(typo): exernal => external Signed-off-by: Kotaro Yoshimoto * fix(typo): undderlying => underlying Signed-off-by: Kotaro Yoshimoto * fix(typo): expicitly => explicitly Signed-off-by: Kotaro Yoshimoto * fix(typo): paremterized => parameterized Signed-off-by: Kotaro Yoshimoto * fix(typo): thier => their Signed-off-by: Kotaro Yoshimoto * fix(typo): simualtor => simulator Signed-off-by: Kotaro Yoshimoto * fix(typo): modifiy => modify Signed-off-by: Kotaro Yoshimoto * fix(typo): neccessary => necessary Signed-off-by: Kotaro Yoshimoto * fix(typo): travelled => traveled Signed-off-by: Kotaro Yoshimoto * fix(typo): heursitic => heuristic Signed-off-by: Kotaro Yoshimoto * fix(typo): chagne => change Signed-off-by: Kotaro Yoshimoto * fix(typo): waypints => waypoints Signed-off-by: Kotaro Yoshimoto * fix(typo): unknwon => unknown Signed-off-by: Kotaro Yoshimoto * fix(typo): true => true Signed-off-by: Kotaro Yoshimoto * fix(typo): approximiate => approximate Signed-off-by: Kotaro Yoshimoto * fix(typo): analitically => analytically Signed-off-by: Kotaro Yoshimoto * fix(typo): modify url including typo in original url Signed-off-by: Kotaro Yoshimoto * fix(typo): computationall => computational Signed-off-by: Kotaro Yoshimoto * fix(typo): hypothenus => hypotenuse Signed-off-by: Kotaro Yoshimoto * fix(typo): neighbour => neighbor Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * fix(typo): modify url including typo in original url Signed-off-by: Kotaro Yoshimoto * fix(typo): kiro => kilo Signed-off-by: Kotaro Yoshimoto * fix(typo): flowchar => flowchart Signed-off-by: Kotaro Yoshimoto * fix(typo): projecton => projection Signed-off-by: Kotaro Yoshimoto * fix(cspell): divide variable name with space to fix cspell error Signed-off-by: Kotaro Yoshimoto * fix(typo): yawrate => yaw rate Signed-off-by: Kotaro Yoshimoto * fix(typo): timelag => time_lag Signed-off-by: Kotaro Yoshimoto * fix(cspell): divide variable name with space to fix cspell error Signed-off-by: Kotaro Yoshimoto * fix(typo): retrive => retrieve Signed-off-by: Kotaro Yoshimoto * fix(typo): posemsg => pose msg Signed-off-by: Kotaro Yoshimoto * fix(cspell): replace northup with east_north_up Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * fix(cspell): ignore person names Signed-off-by: Kotaro Yoshimoto * fix(cspell): ignore cspell error due to the source from OpenCV Signed-off-by: Kotaro Yoshimoto * fix(cspell): ignore cspell error due to the source from OpenCV Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * chore(spell-check): ignore minx, maxx, miny, maxy, minz, maxz from autoware parameter names Signed-off-by: Kotaro Yoshimoto * chore(spell-check): Ignore cspell errors caused by external factor(plotjuggler) Signed-off-by: Kotaro Yoshimoto * fix(typo): dereferencable => dereferenceable Signed-off-by: Kotaro Yoshimoto * fix(typo): maxs => maxes Signed-off-by: Kotaro Yoshimoto * fix(typo): interpolatable => interpolable (more common) Signed-off-by: Kotaro Yoshimoto * fix(typo): fillter => filter Signed-off-by: Kotaro Yoshimoto * fix(typo): retrurn => return Signed-off-by: Kotaro Yoshimoto * fix(typo): diagnotics => diagnostics Signed-off-by: Kotaro Yoshimoto * fix(typo): Frist => First Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore ptfilter (external reference code) Signed-off-by: Kotaro Yoshimoto * fix(typo): overwite => overwrite Signed-off-by: Kotaro Yoshimoto * fix(cspell): use semi-major instead of semimajor Signed-off-by: Kotaro Yoshimoto * fix(typo): transien => transient Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore multipolygon, multilinestring Signed-off-by: Kotaro Yoshimoto * fix(typo): symetric => symmetric Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore Gammell (person name) Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore Karaman (person name) Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore feps with adding explanation Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace iradius with i_radius Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace inorm with inv_norm Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace idist with i_dist Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore lfit, LFIT Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore Bboxes Signed-off-by: Kotaro Yoshimoto * fix(typo): unsuppoerted => unsupported Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore person names Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace eigvec with eig_vec Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace eigv with eig_v Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore eigenbox Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace fltmax with flt_max Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore asan Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore rsspace with adding explanation Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace bfqueue with bf_queue Signed-off-by: Kotaro Yoshimoto * chore(cspell): expanded abbreviations in variable names　in debug_plot.py Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore nparr with adding explanation Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace vmodel with vehicle_model Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore fpalgos Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace inpro with inner_product Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace iradius with i_radius Signed-off-by: Kotaro Yoshimoto * chore(cspell): replace sstm with ss Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore dend Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore ndim, ndata, linewidth Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore errors from parameter name Signed-off-by: Kotaro Yoshimoto * fix(typo): socre => score Signed-off-by: Kotaro Yoshimoto * chore(cspell): newstamp => new_stamp Signed-off-by: Kotaro Yoshimoto * chore(cspell): fuseon => fuseOn Signed-off-by: Kotaro Yoshimoto * chore(cspell): stdpair => std_pair Signed-off-by: Kotaro Yoshimoto * chore(cspell): boxid => box_id Signed-off-by: Kotaro Yoshimoto * fix(typo): intensity => intensity Signed-off-by: Kotaro Yoshimoto * fix(typo): inorder to => in order to Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore divup Signed-off-by: Kotaro Yoshimoto * chore(cspell): faceobjects => face_objects Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore rsspace Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore errors from citation Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore moraisim Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore ADMM Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore pointinpoly from reference Signed-off-by: Kotaro Yoshimoto * fix(typo): replaned => replanned Signed-off-by: Kotaro Yoshimoto * fix(typo): interaface => interface Signed-off-by: Kotaro Yoshimoto * fix(typo): supress => suppress Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * fix(typo): distane => distance Signed-off-by: Kotaro Yoshimoto * fix(typo): relevent => relevant Signed-off-by: Kotaro Yoshimoto * fix(typo): pedestrain => pedestrian Signed-off-by: Kotaro Yoshimoto * fix(typo): obejct => object Signed-off-by: Kotaro Yoshimoto * fix(typo): paramters => parameters Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore asdasd Signed-off-by: Kotaro Yoshimoto * chore(cspell): unnormalized => un-normalized Signed-off-by: Kotaro Yoshimoto * chore(cspell): precompilation => pre-compilation Signed-off-by: Kotaro Yoshimoto * fix(typo): compensents => components Signed-off-by: Kotaro Yoshimoto * fix(typo): cummulative => cumulative Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore degrounded Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore person names Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * chore(cspell): publically => publicly Signed-off-by: Kotaro Yoshimoto * chore(cspell): interpolable => interpolatable Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore longl Signed-off-by: Kotaro Yoshimoto * chore(cspell): pngs => png images Signed-off-by: Kotaro Yoshimoto * chore(cspell): concate => concat Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore cand Signed-off-by: Kotaro Yoshimoto * chore(cspell): image magick => imagemagick Signed-off-by: Kotaro Yoshimoto * fix(typo): faceo_ject=> face_object Signed-off-by: Kotaro Yoshimoto * chore(cspell): velocityinsertion => velocity insertion Signed-off-by: Kotaro Yoshimoto * fix(typo): euclidian => euclidean Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore steerings Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore OCCUPANCYGRID Signed-off-by: Kotaro Yoshimoto * fix(typo): occuring => occurring Signed-off-by: Kotaro Yoshimoto * fix(typo): refere => refer Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore fourcell Signed-off-by: Kotaro Yoshimoto * chore(cspell): eigvalue => eigenvalue Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore badpt Signed-off-by: Kotaro Yoshimoto * chore(cspell): ignore divb Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * style(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * doc: add comment to describe LFIT Co-authored-by: Yukihiro Saito Signed-off-by: Kotaro Yoshimoto * fix(typo): computationall => computational Signed-off-by: Kotaro Yoshimoto * fix(typo): hypothenus => hypotenuse Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * fix(typo): computationall => computational Signed-off-by: Kotaro Yoshimoto * fix(typo): hypothenus => hypotenuse Signed-off-by: Kotaro Yoshimoto * ci(pre-commit): autofix Signed-off-by: Kotaro Yoshimoto * update Signed-off-by: Kotaro Yoshimoto * fix(typo): interpolatable => interpolable (more common) Signed-off-by: Kotaro Yoshimoto * Squashed commit of the following: commit c7d3b7d2132323af3437af01e9d774b13005bace Author: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Date: Fri Dec 16 13:51:35 2022 +0900 test(freespace_planning_algorithms): done't dump rosbag by default (#2504) Signed-off-by: Hirokazu Ishida Signed-off-by: Hirokazu Ishida commit 6731e0ced39e3187c2afffe839eaa697a19e5e84 Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri Dec 16 09:29:35 2022 +0900 feat(pose_initializer): partial map loading (#2500) * first commit Signed-off-by: kminoda * move function Signed-off-by: kminoda * now works Signed-off-by: kminoda * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * clarify how to enable partial mao loading interface Signed-off-by: kminoda * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * Update localization/pose_initializer/config/pose_initializer.param.yaml Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> * fix pre-commit Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> commit efb4ff1cea6e07aa9e894a6042e8685e30b420ba Author: Kosuke Takeuchi Date: Thu Dec 15 17:29:44 2022 +0900 feat(trajectory_follower): extend mpc trajectory for terminal yaw (#2447) * feat(trajectory_follower): extend mpc trajectory for terminal yaw Signed-off-by: kosuke55 * make mpc min vel param Signed-off-by: kosuke55 * add mpc extended point after smoothing Signed-off-by: kosuke55 * Revert "make mpc min vel param" This reverts commit 02157b6ae0c2ff1564840f6d15e3c55025327baf. Signed-off-by: kosuke55 * add comment and hypot Signed-off-by: kosuke55 * remove min vel Signed-off-by: kosuke55 * add flag for extending traj Signed-off-by: kosuke55 * add extend param to default param Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 * fix from TakaHoribe review Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 * refactor Signed-off-by: kosuke55 Signed-off-by: kosuke55 commit ad2ae7827bdc3af7da8607fdd53ea74940426421 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Thu Dec 15 15:52:34 2022 +0900 feat(component_interface_tools): add service log checker (#2503) * feat(component_interface_utils): add service log checker Signed-off-by: Takagi, Isamu * feat(component_interface_tools): add service log checker Signed-off-by: Takagi, Isamu * feat(component_interface_tools): add diagnostics Signed-off-by: Takagi, Isamu * feat: update system error monitor config Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 4a13cc5a32898f5b17791d9381744bf71ff8ed20 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu Dec 15 12:54:11 2022 +0900 fix(behavior_path_planner): fix goal lanelet extension (#2508) Signed-off-by: yutaka Signed-off-by: yutaka commit 77b1c36b5ca89b25250dcbb117c9f03a9c36c1c4 Author: Kyoichi Sugahara <81.s.kyo.19@gmail.com> Date: Thu Dec 15 10:45:45 2022 +0900 feat(behavior_path_planner): change side shift module logic (#2195) * change side shift module design Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * cherry picked side shift controller Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * add debug marker to side shift Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * fix pointer error due to direct assignment added make_shared Signed-off-by: Muhammad Zulfaqar Azmi * add flow chart Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * add status of AFTER_SHIFT Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * remove function for debug Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * ci(pre-commit): autofix * fix flow chart Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * ci(pre-commit): autofix Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: tanaka3 Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 9183c4f20eb4592ed0b48c2eac67add070711677 Author: Takamasa Horibe Date: Wed Dec 14 19:59:00 2022 +0900 refactor(simple_planning_simulator): make function for duplicated code (#2502) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe commit ed992b10ed326f03354dce3b563b8622f9ae9a6c Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 14 17:48:24 2022 +0900 fix(behavior_path_planner): fix planner data copy (#2501) Signed-off-by: yutaka Signed-off-by: yutaka commit 0c6c46b33b3c828cb95eaa31fcbf85655fc6a55f Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 14 14:42:16 2022 +0900 fix(behavior_path_planner): fix find nearest function from lateral distance (#2499) * feat(behavior_path_planner): fix find nearest function from lateral distance * empty commit commit a26b69d1df55e9369ea3adcdd011ae2d7c86dfb7 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 14 11:28:07 2022 +0900 feat(behavior_path_planner): fix overlap checker (#2498) * feat(behavior_path_planner): fix overlap checker Signed-off-by: yutaka * remove reserve Signed-off-by: yutaka Signed-off-by: yutaka commit 3a24859ca6851caaeb25fc4fac2334fcbdb887d1 Author: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Tue Dec 13 16:51:59 2022 +0300 feat(mission_planner): check goal footprint (#2088) Signed-off-by: ismet atabay commit b6a18855431b5f3a67fcbf383fac8df2b45d462e Author: Takamasa Horibe Date: Tue Dec 13 22:46:24 2022 +0900 feat(trajectory_visualizer): update for steer limit, remove tf for pose source (#2267) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe commit f1a9a9608559a5b89f631df3dc2fadd037e36ab4 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue Dec 13 19:47:16 2022 +0900 feat(behavior_path_planner): remove unnecessary code and clean turn signal decider (#2494) * feat(behavior_path_planner): clean drivable area code Signed-off-by: yutaka * make a function for turn signal decider Signed-off-by: yutaka Signed-off-by: yutaka commit fafe1d8235b99302bc9ba8f3770ae34878f1e7e7 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue Dec 13 18:19:41 2022 +0900 feat(behavior_path_planner): change turn signal output timing (#2493) Signed-off-by: yutaka Signed-off-by: yutaka commit c48b9cfa7074ecd46d96f6dc43679e17bde3a63d Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue Dec 13 09:16:14 2022 +0900 feat(map_loader): add differential map loading interface (#2417) * first commit Signed-off-by: kminoda * ci(pre-commit): autofix * added module load in _node.cpp Signed-off-by: kminoda * ci(pre-commit): autofix * create pcd metadata dict when either of the flag is true Signed-off-by: kminoda * ci(pre-commit): autofix * fix readme * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 9a3613bfcd3e36e522d0ea9130f6200ca7689e2b Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Tue Dec 13 08:49:23 2022 +0900 docs(default_ad_api): add readme (#2491) * docs(default_ad_api): add readme Signed-off-by: Takagi, Isamu * feat: update table Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 49aa10b04de61c36706f6151d11bf17257ca54d1 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Tue Dec 13 06:46:20 2022 +0900 feat(default_ad_api): split parameters into file (#2488) * feat(default_ad_api): split parameters into file Signed-off-by: Takagi, Isamu * feat: remove old parameter Signed-off-by: Takagi, Isamu * fix: test Signed-off-by: Takagi, Isamu * feat: add default config Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 7f0138c356c742b6e15e571e7a4683caa55969ac Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon Dec 12 22:16:54 2022 +0900 feat(behavior_path_planner, obstacle_avoidance_planner): add new drivable area (#2472) * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update obstacle avoidance planner Signed-off-by: yutaka * update Signed-off-by: yutaka * clean code Signed-off-by: yutaka * uddate Signed-off-by: yutaka * clean code Signed-off-by: yutaka * remove resample Signed-off-by: yutaka * update Signed-off-by: yutaka * add orientation Signed-off-by: yutaka * change color Signed-off-by: yutaka * update Signed-off-by: yutaka * remove drivable area Signed-off-by: yutaka * add flag Signed-off-by: yutaka * update Signed-off-by: yutaka * update color Signed-off-by: yutaka * fix some codes Signed-off-by: yutaka * change to makerker array Signed-off-by: yutaka * change avoidance utils Signed-off-by: yutaka Signed-off-by: yutaka commit c855e23cc17d1518ebce5dd15629d03acfe17da3 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Mon Dec 12 17:15:10 2022 +0900 refactor(vehicle_cmd_gate): remove old emergency topics (#2403) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit fa04d540c9afdded016730c9978920a194d2d2b4 Author: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon Dec 12 16:04:00 2022 +0900 feat: replace python launch with xml launch for system monitor (#2430) * feat: replace python launch with xml launch for system monitor Signed-off-by: Daisuke Nishimatsu * ci(pre-commit): autofix * update figure Signed-off-by: Daisuke Nishimatsu Signed-off-by: Daisuke Nishimatsu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 4a6990c49d1f8c3bedfb345e7c94c3c6893b4099 Author: Kosuke Takeuchi Date: Mon Dec 12 15:01:39 2022 +0900 feat(trajectory_follower): pub steer converged marker (#2441) * feat(trajectory_follower): pub steer converged marker Signed-off-by: kosuke55 * Revert "feat(trajectory_follower): pub steer converged marker" This reverts commit a6f6917bc542d5b533150f6abba086121e800974. Signed-off-by: kosuke55 * add steer converged debug marker in contoller_node Signed-off-by: kosuke55 Signed-off-by: kosuke55 commit 3c01f15125dfbc45e1050ee96ccc42618d6ee6fd Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Mon Dec 12 12:48:41 2022 +0900 docs(tier4_state_rviz_plugin): update readme (#2475) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit d8ece0040354be5381a27403bcc757354735a77b Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Mon Dec 12 11:57:03 2022 +0900 chore(simulator_compatibility_test): suppress setuptools warnings (#2483) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 727586bfe86dc9cb21ce34d9cbe19c241e162b04 Author: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon Dec 12 10:00:35 2022 +0900 fix(behavior_path_planner): lane change candidate resolution (#2426) * fix(behavior_path_planner): lane change candidate resolution Signed-off-by: Muhammad Zulfaqar Azmi * rework sampling based on current speed Signed-off-by: Muhammad Zulfaqar Azmi * refactor code Signed-off-by: Muhammad Zulfaqar Azmi * use util's resampler Signed-off-by: Muhammad Zulfaqar Azmi * consider min_resampling_points and resampling dt Signed-off-by: Muhammad Zulfaqar * simplify code Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar commit 284548ca7f38b1d83af11f2b9caaac116eb9b09c Author: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon Dec 12 09:57:19 2022 +0900 fix(behavior_path_planner): minimum distance for lane change (#2413) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi commit 469d8927bd7a0c98b9d491d347e111065973e13f Author: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Fri Dec 9 21:27:18 2022 +0900 revert(behavior_path): revert removal of refineGoalFunction (#2340)" (#2485) This reverts commit 8e13ced6dfb6edfea77a589ef4cb93d82683bf51. Signed-off-by: mitsudome-r Signed-off-by: mitsudome-r commit d924f85b079dfe64feab017166685be40e977e62 Author: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri Dec 9 19:53:51 2022 +0800 fix(freespace_planning_algorithms): fix rrtstar can't arrive goal error (#2350) Signed-off-by: NorahXiong Signed-off-by: NorahXiong Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> commit b2ded82324bce78d9db3ff01b0227b00709b1efe Author: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri Dec 9 17:12:13 2022 +0900 fix(ground-segmentation): recheck gnd cluster pointcloud (#2448) * fix: reclassify ground cluster pcl Signed-off-by: badai-nguyen * fix: add lowest-based recheck Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> commit 8906a1e78bc5b7d6417683ecedc1efe3f48be31e Author: Takamasa Horibe Date: Fri Dec 9 16:29:45 2022 +0900 fix(trajectory_follower): fix mpc trajectory z pos (#2482) Signed-off-by: takahoribe Signed-off-by: takahoribe commit d4939058f05f9a1609f0ed22afbd0d4febfb212d Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri Dec 9 12:40:30 2022 +0900 feat(behavior_velocity_planner): clean walkway module (#2480) Signed-off-by: yutaka Signed-off-by: yutaka commit d3b86a37ae7c3a0d59832caf56afa13b148d562c Author: Makoto Kurihara Date: Thu Dec 8 22:59:32 2022 +0900 fix(emergency_handler): fix mrm handling when mrm behavior is none (#2476) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara commit 2dde073a101e96757ef0cd189bb9ff06836934e9 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Thu Dec 8 17:16:13 2022 +0900 feat(behavior_velocity_planner): add velocity factors (#1985) * (editting) add intersection_coordination to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add intersection coordination to stop reasons Signed-off-by: TakumiKozaka-T4 * (Editting) add v2x to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 publisher Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 to scene modules Signed-off-by: TakumiKozaka-T4 * add stop reason2 to obstacle stop planner and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * Modify files including unintended change by rebase Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * Modification 1: not to publsh vacant stop reason, 2: change default status in obstacle stop and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * fix error Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * modification for renaming stop_reason2 to motion_factor Signed-off-by: TakumiKozaka-T4 * (Editting) rename variables Signed-off-by: TakumiKozaka-T4 * bug fix Signed-off-by: TakumiKozaka-T4 * (WIP) Add motion factor message. Modify scene modules due to new motion factor. Moving motion factor aggregator. Signed-off-by: TakumiKozaka-T4 * (WIP) Save current work. Modify aggregator, CMakeList. Add launcher Signed-off-by: TakumiKozaka-T4 * (WIP) Solved build error, but not launched Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing launch error Signed-off-by: TakumiKozaka-T4 * Fix error in launching motion factor aggregator Signed-off-by: TakumiKozaka-T4 * Delete unnecessary comment-out in CMakelists. Change remapping in launcher. Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * pull the latest foundation/main Signed-off-by: TakumiKozaka-T4 * (fix for pre-commit.ci) Add to motion_factor_aggregator.hpp Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * feat: add velocity factor interface Signed-off-by: Takagi, Isamu * fix: fix build error Signed-off-by: Takagi, Isamu * feat: stop sign Signed-off-by: Takagi, Isamu * WIP Signed-off-by: Takagi, Isamu * feat: update visualizer Signed-off-by: Takagi, Isamu * feat: modify traffic light manager Signed-off-by: Takagi, Isamu * feat: update velocity factors Signed-off-by: Takagi, Isamu * feat: update api Signed-off-by: Takagi, Isamu * feat: move adapi msgs Signed-off-by: Takagi, Isamu * feat: remove old aggregator Signed-off-by: Takagi, Isamu * feat: move api Signed-off-by: Takagi, Isamu * feat: rename message Signed-off-by: Takagi, Isamu * feat: add using Signed-off-by: Takagi, Isamu * feat: add distance Signed-off-by: Takagi, Isamu * feat: fix build error Signed-off-by: Takagi, Isamu * feat: use nan as default distance Signed-off-by: Takagi, Isamu * fix: set virtual traffic light detail Signed-off-by: Takagi, Isamu * fix: remove debug code Signed-off-by: Takagi, Isamu * fix: copyright Signed-off-by: Takagi, Isamu Signed-off-by: TakumiKozaka-T4 Signed-off-by: Takagi, Isamu Co-authored-by: TakumiKozaka-T4 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 9a5057e4948ff5ac9165c14eb7112d79f2de76d5 Author: Kosuke Takeuchi Date: Thu Dec 8 13:42:50 2022 +0900 fix(freespace_planning_algorithms): comment out failing tests (#2440) Signed-off-by: kosuke55 Signed-off-by: kosuke55 commit cddb8c74d0fbf49390b4d462c20c12bc257f4825 Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu Dec 8 11:57:04 2022 +0900 feat(gyro_odometer): publish twist when both data arrives (#2423) * feat(gyro_odometer): publish when both data arrive Signed-off-by: kminoda * remove unnecessary commentouts Signed-off-by: kminoda * ci(pre-commit): autofix * use latest timestamp Signed-off-by: kminoda * small fix Signed-off-by: kminoda * debugged Signed-off-by: kminoda * update gyro_odometer Signed-off-by: kminoda * ci(pre-commit): autofix * add comments Signed-off-by: kminoda * add comments Signed-off-by: kminoda * ci(pre-commit): autofix * fix timestamp validation flow Signed-off-by: kminoda * ci(pre-commit): autofix * remove unnecessary commentouts Signed-off-by: kminoda * pre-commit Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit f0f513cf44532dfe8d51d27c4caef23fb694af16 Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu Dec 8 11:08:29 2022 +0900 fix: remove unnecessary DEBUG_INFO declarations (#2457) Signed-off-by: kminoda Signed-off-by: kminoda commit 01daebf42937a05a2d83f3dee2c0778389492e50 Author: Takayuki Murooka Date: Thu Dec 8 00:28:35 2022 +0900 fix(tier4_autoware_api_launch): add rosbridge_server dependency (#2470) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka commit 26ef8174b1c12b84070b36df2a7cd14bfa9c0363 Author: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Wed Dec 7 19:32:09 2022 +0900 fix: rename `use_external_emergency_stop` to `check_external_emergency_heartbeat` (#2455) * fix: rename use_external_emergency_stop to check_external_emergency_heartbeat * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 024b993a0db8c0d28db0f05f64990bed7069cbd8 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 7 18:00:32 2022 +0900 fix(motion_utils): rename sampling function (#2469) Signed-off-by: yutaka Signed-off-by: yutaka commit c240ce2b6f4e79c435ed651b347a7d665a947862 Author: Yukihiro Saito Date: Wed Dec 7 16:33:44 2022 +0900 feat: remove web controller (#2405) Signed-off-by: Yukihiro Saito Signed-off-by: Yukihiro Saito commit 2992b1cadae7e7ac86fd249998ce3c7ddbe476c9 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 7 15:39:28 2022 +0900 feat(motion_utils): add points resample function (#2465) Signed-off-by: yutaka Signed-off-by: yutaka commit 4a75d7c0ddbd88f54afaf2bb05eb65138a53ea60 Author: Mingyu1991 <115005477+Mingyu1991@users.noreply.github.com> Date: Wed Dec 7 14:42:33 2022 +0900 docs: update training data for traffic light (#2464) * update traffic light cnn classifier README.md * correct to upper case Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> commit a4287165be87fa7727f79c01dfb0bea6af54c333 Author: Ryuta Kambe Date: Wed Dec 7 12:21:49 2022 +0900 perf(behavior_velocity_planner): remove unnecessary debug data (#2462) Signed-off-by: veqcc commit 0a5b2857d3b2c1c9370598013b25aeaebf2d654d Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 7 12:03:46 2022 +0900 feat(behavior_path_planner): cut overlapped path (#2451) * feat(behavior_path_planner): cut overlapped path Signed-off-by: yutaka * clean code Signed-off-by: yutaka Signed-off-by: yutaka commit 65003dc99f2abe937afcc010514530fa666fbbfd Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Wed Dec 7 11:06:41 2022 +0900 revert(default_ad_api): fix autoware state to add wait time (#2407) (#2460) Revert "fix(default_ad_api): fix autoware state to add wait time (#2407)" This reverts commit c4224854a7e57a9526dde998f742741fe383471c. commit fab18677ca4de378faff84a41db5147577e7448d Author: Makoto Kurihara Date: Wed Dec 7 10:32:41 2022 +0900 fix(raw_vehicle_cmd_converter): fix column index for map validation (#2450) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara commit a1d3c80a4f5e3a388887a5afb32d9bf7961301f1 Author: Ambroise Vincent Date: Tue Dec 6 10:39:02 2022 +0100 fix(tvm_utility): copy test result to CPU (#2414) Also remove dependency to autoware_auto_common. Issue-Id: SCM-5401 Signed-off-by: Ambroise Vincent Change-Id: I83b859742df2f2ff7df1d0bd2d287bfe0aa04c3d Signed-off-by: Ambroise Vincent Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> commit eb9946832c7e42d5380fd71956165409d0b592c3 Author: Mamoru Sobue Date: Tue Dec 6 18:11:41 2022 +0900 chore(behaviror_velocity_planner): changed logging level for intersection (#2459) changed logging level Signed-off-by: Mamoru Sobue commit c4224854a7e57a9526dde998f742741fe383471c Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Tue Dec 6 17:01:37 2022 +0900 fix(default_ad_api): fix autoware state to add wait time (#2407) * fix(default_ad_api): fix autoware state to add wait time Signed-off-by: Takagi, Isamu * Update system/default_ad_api/src/compatibility/autoware_state.cpp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: Takagi, Isamu Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> commit f984fbb708cb02947ec2824ce041c739c35940f7 Author: Takamasa Horibe Date: Tue Dec 6 13:55:17 2022 +0900 feat(transition_manager): add param to ignore autonomous transition condition (#2453) * feat(transition_manager): add param to ignore autonomous transition condition Signed-off-by: Takamasa Horibe * same for modeChangeCompleted Signed-off-by: Takamasa Horibe * remove debug print Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe commit d3e640df270a0942c4639e11451faf26e099bbe1 Author: Tomoya Kimura Date: Tue Dec 6 13:01:06 2022 +0900 feat(operation_mode_transition_manager): transition to auto quickly when vehicle stops (#2427) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura Signed-off-by: Kotaro Yoshimoto * chore(cspell): interpolable => interpolatable Signed-off-by: Kotaro Yoshimoto * Revert "Merge branch 'destroy-typos-check-all' into destroy-typos" This reverts commit 6116ca02d9df59f815d772a271fed7b0b21ebaf7, reversing changes made to 1f7157a6b6d957dc0ddd2ac5ef7f8a36c94b96e4. Signed-off-by: Kotaro Yoshimoto * chore: fix duplication of parameter Signed-off-by: Kotaro Yoshimoto * chore: fix duplication of function Signed-off-by: Kotaro Yoshimoto * revert: system/system_monitor/launch/system_monitor.launch.xml Signed-off-by: Kotaro Yoshimoto --------- Signed-off-by: Kotaro Yoshimoto Signed-off-by: Kotaro Yoshimoto Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yukihiro Saito --- .../helper_functions/message_adapters.hpp | 2 +- .../test/test_template_utils.cpp | 1 + .../bounding_box/bounding_box_common.hpp | 4 +- .../geometry/bounding_box/eigenbox_2d.hpp | 47 ++++++++------- .../include/geometry/bounding_box/lfit.hpp | 18 +++--- .../bounding_box/rotating_calipers.hpp | 6 +- .../include/geometry/common_2d.hpp | 9 +-- .../include/geometry/convex_hull.hpp | 2 + .../include/geometry/intersection.hpp | 7 ++- .../include/geometry/interval.hpp | 6 +- .../include/geometry/spatial_hash_config.hpp | 30 +++++----- .../src/bounding_box.cpp | 4 +- .../test/include/test_bounding_box.hpp | 5 +- .../test/src/lookup_table.cpp | 2 +- .../test/src/test_area.cpp | 2 +- .../test/src/test_common_2d.cpp | 2 +- .../test/test_polygon_iterator.cpp | 4 +- .../src/spline_interpolation_points_2d.cpp | 2 +- .../documentation/ButterworthFilter.md | 2 + common/signal_processing/src/butterworth.cpp | 1 + .../tensorrt_common/src/tensorrt_common.cpp | 1 + .../control_performance_analysis/README.md | 4 +- .../config/controller_monitor.xml | 2 +- .../control_performance_analysis_utils.cpp | 8 +-- control/mpc_lateral_controller/README.md | 2 + .../mpc_lateral_controller/mpc_trajectory.hpp | 2 +- .../mpc_lateral_controller/mpc_utils.hpp | 2 +- .../qp_solver/qp_solver_interface.hpp | 2 +- .../test/test_interpolate.cpp | 2 +- .../pure_pursuit/util/planning_utils.hpp | 1 + .../longitudinal_controller_base.hpp | 4 +- localization/ekf_localizer/README.md | 2 +- localization/ndt_scan_matcher/README.md | 2 + .../config/ndt_scan_matcher.param.yaml | 1 + .../ndt_scan_matcher_core.hpp | 1 + .../src/ndt_scan_matcher_core.cpp | 1 + .../test/test_uniform_random.cpp | 2 +- perception/elevation_map_loader/README.md | 2 +- .../include/ground_segmentation/gencolors.hpp | 2 + .../heatmap_visualizer_node.hpp | 2 +- .../config/pointpainting.param.yaml | 2 +- .../docs/pointpainting-fusion.md | 4 ++ .../fusion_node.hpp | 5 +- .../launch/roi_cluster_fusion.launch.xml | 1 + .../src/fusion_node.cpp | 49 ++++++++-------- .../pointpainting_fusion/preprocess_kernel.cu | 1 + .../pointpainting_fusion/voxel_generator.cpp | 4 +- .../src/roi_cluster_fusion/node.cpp | 2 + .../src/roi_detected_object_fusion/node.cpp | 2 + .../lidar_centerpoint/centerpoint_config.hpp | 2 +- .../include/lidar_centerpoint/utils.hpp | 1 + .../centerpoint_vs_centerpoint-tiny/README.md | 2 +- .../lib/network/scatter_kernel.cu | 1 + .../lib/postprocess/circle_nms_kernel.cu | 1 + .../lib/postprocess/postprocess_kernel.cu | 3 +- .../lib/preprocess/preprocess_kernel.cu | 1 + .../lib/preprocess/voxel_generator.cpp | 4 +- perception/lidar_centerpoint/lib/utils.cpp | 1 + .../include/lidar_centerpoint_tvm/utils.hpp | 1 + .../postprocess/generate_detected_boxes.cpp | 11 ++-- .../lib/preprocess/generate_features.cpp | 3 +- .../lidar_centerpoint_tvm/lib/utils.cpp | 1 + perception/map_based_prediction/README.md | 2 +- .../multi_object_tracker/utils/utils.hpp | 2 +- ...dar_fusion_to_detected_object_5.drawio.svg | 4 +- perception/tensorrt_yolox/README.md | 2 + .../include/tensorrt_yolox/tensorrt_yolox.hpp | 5 +- .../tensorrt_yolox/src/tensorrt_yolox.cpp | 29 +++++----- .../avoidance/avoidance_module.cpp | 4 +- .../crosswalk-design.md | 2 +- .../docs/crosswalk/limitation.svg | 4 +- planning/freespace_planner/README.md | 6 +- .../freespace_planning_algorithms/README.md | 4 +- .../freespace_planning_algorithms/rrtstar.hpp | 4 +- .../rrtstar_core.hpp | 9 +-- .../freespace_planning_algorithms/rrtstar.md | 6 +- .../src/rrtstar.cpp | 16 +++--- .../src/rrtstar_core.cpp | 57 ++++++++++--------- .../test/debug_plot.py | 43 +++++++------- .../test/debug_plot_rrtstar_core.py | 2 + .../test_freespace_planning_algorithms.cpp | 17 +++--- planning/obstacle_cruise_planner/README.md | 2 +- .../pid_based_planner/pid_based_planner.hpp | 2 +- planning/obstacle_velocity_limiter/README.md | 4 +- .../collision_checker_benchmark.cpp | 1 + .../obstacle_velocity_limiter/debug.hpp | 1 + .../obstacle_velocity_limiter/distance.hpp | 1 + .../forward_projection.hpp | 1 + .../obstacle_velocity_limiter/map_utils.hpp | 1 + .../obstacle_velocity_limiter.hpp | 5 +- .../obstacle_velocity_limiter_node.hpp | 1 + .../obstacle_velocity_limiter/obstacles.hpp | 1 + .../occupancy_grid_utils.hpp | 1 + .../obstacle_velocity_limiter/parameters.hpp | 2 + .../pointcloud_utils.hpp | 3 +- .../trajectory_preprocessing.hpp | 1 + .../obstacle_velocity_limiter/types.hpp | 1 + .../obstacle_velocity_limiter/src/debug.cpp | 1 + .../src/distance.cpp | 8 +-- .../src/forward_projection.cpp | 1 + .../src/map_utils.cpp | 1 + .../src/obstacle_velocity_limiter_node.cpp | 1 + .../src/obstacles.cpp | 1 + .../src/occupancy_grid_utils.cpp | 1 + .../src/pointcloud_utils.cpp | 1 + .../test/test_collision_distance.cpp | 1 + .../test/test_forward_projection.cpp | 1 + .../test/test_obstacles.cpp | 1 + .../test/test_occupancy_grid_utils.cpp | 1 + planning/planning_debug_tools/Readme.md | 3 + .../scripts/closest_velocity_checker.py | 4 +- .../include/planning_validator/utils.hpp | 1 + sensing/geo_pos_conv/src/geo_pos_conv.cpp | 2 +- .../gnss_poser/include/gnss_poser/convert.hpp | 16 +++--- .../include/gnss_poser/gnss_stat.hpp | 4 +- sensing/image_diagnostics/README.md | 2 +- .../config/processing_time_ms.xml | 2 +- .../docs/blockage_diag.md | 2 +- .../docs/distortion-corrector.md | 2 +- .../image/blockage_diag_flowchart.drawio.svg | 10 ++-- .../outlier_filter-return_type.drawio.svg | 4 +- .../passthrough_filter/passthrough_uint16.hpp | 1 + .../dual_return_outlier_filter_nodelet.cpp | 6 +- .../passthrough_filter/passthrough_uint16.cpp | 9 +-- .../voxel_grid_nearest_centroid.hpp | 13 +++-- .../voxel_grid_nearest_centroid_impl.hpp | 3 +- .../src/signed_distance_function.cpp | 2 +- .../simulator_compatibility_test/README.md | 4 +- .../simulator_compatibility_test/setup.py | 2 + .../publishers/moraisim/morai_ctrl_cmd.py | 1 + .../accel_brake_map_calibrator/README.md | 3 + .../raw_vehicle_cmd_converter/src/node.cpp | 2 +- 132 files changed, 394 insertions(+), 272 deletions(-) diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/helper_functions/message_adapters.hpp index 4d5dbec213aa8..352ef7c7b65d5 100644 --- a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/helper_functions/message_adapters.hpp @@ -34,7 +34,7 @@ namespace message_field_adapters /// Using alias for Time message using TimeStamp = builtin_interfaces::msg::Time; -/// \brief Helper class to check existance of header file in compile time: +/// \brief Helper class to check existence of header file in compile time: /// https://stackoverflow.com/a/16000226/2325407 template struct HasHeader : std::false_type diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp index 5ef1dea5b8ad1..9c9ca9ae4b5f2 100644 --- a/common/autoware_auto_common/test/test_template_utils.cpp +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -60,6 +60,7 @@ template using false_bar_expression2 = decltype(std::declval().bar(std::declval(), std::declval())); +// cspell: ignore asdasd // Signature mismatch: template using false_bar_expression3 = decltype(std::declval().asdasd( diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp index 10c8b48f6f546..d1dee63f73b56 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp @@ -107,7 +107,7 @@ template using Point4 = std::array; /// \brief Helper struct for compile time introspection of array size from -/// stackoverflow.com/questions/16866033/getting-the-number-of-elements-in-stdarray-at-compile-time +/// Ref: https://stackoverflow.com/questions/16866033 template struct array_size; template @@ -132,7 +132,7 @@ finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box); /// \brief given support points and two orthogonal directions, compute corners of bounding box /// \tparam PointT Type of a point, must have float members x and y` -/// \tparam IT An iterator dereferencable into PointT +/// \tparam IT An iterator dereferenceable into PointT /// \param[out] corners Gets filled with corner points of bounding box /// \param[in] supports Iterators referring to support points of current bounding box /// e.g. bounding box is touching these points diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp index 9ee460895a0c4..ae142badec4eb 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp @@ -18,6 +18,7 @@ /// \brief This file implements 2D pca on a linked list of points to estimate an oriented /// bounding box +// cspell: ignore eigenbox, EIGENBOX #ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ #define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ @@ -50,11 +51,12 @@ struct Covariance2d std::size_t num_points; }; // struct Covariance2d +// cspell: ignore Welford /// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm /// \param[in] begin An iterator pointing to the first point in a point list /// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferencable into a point with float members x and y -/// \return A 2d covariance matrix for all points inthe list +/// \tparam IT An iterator type dereferenceable into a point with float members x and y +/// \return A 2d covariance matrix for all points in the list template Covariance2d covariance_2d(const IT begin, const IT end) { @@ -93,13 +95,14 @@ Covariance2d covariance_2d(const IT begin, const IT end) /// \brief Compute eigenvectors and eigenvalues /// \param[in] cov 2d Covariance matrix -/// \param[out] eigvec1 First eigenvector -/// \param[out] eigvec2 Second eigenvector +/// \param[out] eig_vec1 First eigenvector +/// \param[out] eig_vec2 Second eigenvector /// \tparam PointT Point type that has at least float members x and y /// \return A pairt of eigenvalues: The first is the larger eigenvalue /// \throw std::runtime error if you would get degenerate covariance template -std::pair eig_2d(const Covariance2d & cov, PointT & eigvec1, PointT & eigvec2) +std::pair eig_2d( + const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2) { const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); @@ -120,28 +123,28 @@ std::pair eig_2d(const Covariance2d & cov, PointT & eigvec // are persistent against further calculations. // (e.g. taking cross product of two eigen vectors) if (fabsf(cov.xy * cov.xy) > std::numeric_limits::epsilon()) { - xr_(eigvec1) = cov.xy; - yr_(eigvec1) = ret.first - cov.xx; - xr_(eigvec2) = cov.xy; - yr_(eigvec2) = ret.second - cov.xx; + xr_(eig_vec1) = cov.xy; + yr_(eig_vec1) = ret.first - cov.xx; + xr_(eig_vec2) = cov.xy; + yr_(eig_vec2) = ret.second - cov.xx; } else { if (cov.xx > cov.yy) { - xr_(eigvec1) = 1.0F; - yr_(eigvec1) = 0.0F; - xr_(eigvec2) = 0.0F; - yr_(eigvec2) = 1.0F; + xr_(eig_vec1) = 1.0F; + yr_(eig_vec1) = 0.0F; + xr_(eig_vec2) = 0.0F; + yr_(eig_vec2) = 1.0F; } else { - xr_(eigvec1) = 0.0F; - yr_(eigvec1) = 1.0F; - xr_(eigvec2) = 1.0F; - yr_(eigvec2) = 0.0F; + xr_(eig_vec1) = 0.0F; + yr_(eig_vec1) = 1.0F; + xr_(eig_vec2) = 1.0F; + yr_(eig_vec2) = 0.0F; } } return ret; } /// \brief Given eigenvectors, compute support (furthest) point in each direction -/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam IT An iterator type dereferenceable into a point with float members x and y /// \tparam PointT type of a point with float members x and y /// \param[in] begin An iterator pointing to the first point in a point list /// \param[in] end An iterator pointing to one past the last point in the point list @@ -183,7 +186,7 @@ bool8_t compute_supports( } /// \brief Compute bounding box given a pair of basis directions -/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam IT An iterator type dereferenceable into a point with float members x and y /// \tparam PointT Point type of the lists, must have float members x and y /// \param[in] ax1 First basis direction, assumed to be normal to ax2 /// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1 @@ -210,7 +213,7 @@ BoundingBox compute_bounding_box( /// modify the list. The resulting bounding box is not necessarily minimum in any way /// \param[in] begin An iterator pointing to the first point in a point list /// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam IT An iterator type dereferenceable into a point with float members x and y /// \return An oriented bounding box in x-y. This bounding box has no height information template BoundingBox eigenbox_2d(const IT begin, const IT end) @@ -222,7 +225,7 @@ BoundingBox eigenbox_2d(const IT begin, const IT end) using PointT = details::base_type; PointT eig1; PointT eig2; - const auto eigv = details::eig_2d(cov, eig1, eig2); + const auto eig_v = details::eig_2d(cov, eig1, eig2); // find extreme points details::Point4 supports; @@ -232,7 +235,7 @@ BoundingBox eigenbox_2d(const IT begin, const IT end) std::swap(eig1, eig2); } BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports); - bbox.value = eigv.first; + bbox.value = eig_v.first; return bbox; } diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp index ac8ed09b4036e..9b8991a7c7132 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp @@ -18,6 +18,8 @@ /// \brief This file implements 2D pca on a linked list of points to estimate an oriented /// bounding box +// cspell: ignore LFIT, lfit +// LFIT means "L-Shape Fitting" #ifndef GEOMETRY__BOUNDING_BOX__LFIT_HPP_ #define GEOMETRY__BOUNDING_BOX__LFIT_HPP_ @@ -117,8 +119,8 @@ float32_t solve_lfit(const LFitWs & ws, PointT & dir) ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)), ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), 0UL}; PointT eig1; - const auto eigv = eig_2d(M, eig1, dir); - return eigv.second; + const auto eig_v = eig_2d(M, eig1, dir); + return eig_v.second; } /// \brief Increments L fit M matrix with information in the point @@ -176,7 +178,7 @@ class LFitCompare /// \param[in] end An iterator pointing to one past the last point in the point list /// \param[in] size The number of points in the point list /// \return A bounding box that minimizes the LFit residual -/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam IT An iterator type dereferenceable into a point with float members x and y template BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size) { @@ -207,11 +209,11 @@ BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::s } } // can recover best corner point, but don't care, need to cover all points - const auto inorm = 1.0F / norm_2d(best_normal); - if (!std::isnormal(inorm)) { + const auto inv_norm = 1.0F / norm_2d(best_normal); + if (!std::isnormal(inv_norm)) { throw std::runtime_error{"LFit: Abnormal norm"}; } - best_normal = times_2d(best_normal, inorm); + best_normal = times_2d(best_normal, inv_norm); auto best_tangent = get_normal(best_normal); // find extreme points Point4 supports; @@ -235,7 +237,7 @@ BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::s /// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list /// \return A pair where the first element is an iterator pointing to the nearest point, and the /// second element is the size of the list -/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam IT An iterator type dereferenceable into a point with float members x and y /// \throw std::domain_error If the number of points is too few template BoundingBox lfit_bounding_box_2d( @@ -258,7 +260,7 @@ BoundingBox lfit_bounding_box_2d( /// \return An oriented bounding box in x-y. This bounding box has no height information /// \param[in] begin An iterator pointing to the first point in a point list /// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam IT An iterator type dereferenceable into a point with float members x and y template BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) { diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp index 9139a2055be12..5bc05810bb1b0 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp @@ -139,7 +139,7 @@ void init_bbox(const IT begin, const IT end, Point4 & support) /// \param[in] end An iterator to one past the last point on a convex hull /// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect /// to -/// \tparam IT An iterator type dereferencable into a point type with float members x and y +/// \tparam IT An iterator type dereferenceable into a point type with float members x and y /// \tparam MetricF A functor that computes a float measure from the x and y size (width and length) /// of a bounding box, assumed to be packed in a Point32 message. /// \throw std::domain_error if the list of points is too small to reasonable generate a bounding @@ -223,7 +223,7 @@ BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF m /// \param[in] begin An iterator to the first point on a convex hull /// \param[in] end An iterator to one past the last point on a convex hull /// \return A minimum area bounding box, value field is the area -/// \tparam IT An iterator type dereferencable into a point type with float members x and y +/// \tparam IT An iterator type dereferenceable into a point type with float members x and y template BoundingBox minimum_area_bounding_box(const IT begin, const IT end) { @@ -238,7 +238,7 @@ BoundingBox minimum_area_bounding_box(const IT begin, const IT end) /// \param[in] begin An iterator to the first point on a convex hull /// \param[in] end An iterator to one past the last point on a convex hull /// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam IT An iterator type dereferencable into a point type with float members x and y +/// \tparam IT An iterator type dereferenceable into a point type with float members x and y template BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end) { diff --git a/common/autoware_auto_geometry/include/geometry/common_2d.hpp b/common/autoware_auto_geometry/include/geometry/common_2d.hpp index 018904d51dda3..fd045003521ea 100644 --- a/common/autoware_auto_geometry/include/geometry/common_2d.hpp +++ b/common/autoware_auto_geometry/include/geometry/common_2d.hpp @@ -262,7 +262,6 @@ T times_2d(const T & p, const float32_t a) /// \tparam T point type. Must have point adapters defined or have float members x and y /// \brief solve p + t * u = q + s * v /// Ref: https://stackoverflow.com/questions/563198/ -/// whats-the-most-efficent-way-to-calculate-where-two-line-segments-intersect /// \param[in] pt anchor point of first line /// \param[in] u direction of first line /// \param[in] q anchor point of second line @@ -274,6 +273,8 @@ inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) { const float32_t num = cross_2d(minus_2d(pt, q), u); float32_t den = cross_2d(v, u); + // cspell: ignore FEPS + // FEPS means "Float EPSilon" constexpr auto FEPS = std::numeric_limits::epsilon(); if (fabsf(den) < FEPS) { if (fabsf(num) < FEPS) { @@ -292,7 +293,7 @@ inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) /// \brief rotate point given precomputed sin and cos /// \param[inout] pt point to rotate /// \param[in] cos_th precomputed cosine value -/// \param[in] sin_th precompined sine value +/// \param[in] sin_th precomputed sine value template inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th) { @@ -321,7 +322,7 @@ inline T rotate_2d(const T & pt, const float32_t th_rad) /// \brief compute q s.t. p T q, or p * q = 0 /// This is the equivalent of a 90 degree ccw rotation /// \param[in] pt point to get normal point of -/// \return point normal to p (unnormalized) +/// \return point normal to p (un-normalized) template inline T get_normal(const T & pt) { @@ -334,7 +335,7 @@ inline T get_normal(const T & pt) /// \tparam T point type. Must have point adapters defined or have float members x and y /// \brief get magnitude of x and y components: /// \param[in] pt point to get magnitude of -/// \return magitude of x and y components together +/// \return magnitude of x and y components together template inline auto norm_2d(const T & pt) { diff --git a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/geometry/convex_hull.hpp index 2687f04f33c7b..e690c4d07441b 100644 --- a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp +++ b/common/autoware_auto_geometry/include/geometry/convex_hull.hpp @@ -136,6 +136,8 @@ typename std::list::const_iterator convex_hull_impl(std::list & const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t { using point_adapter::x_; using point_adapter::y_; + // cspell: ignore FEPS + // FEPS means "Float EPSilon" constexpr auto FEPS = std::numeric_limits::epsilon(); return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b)); }; diff --git a/common/autoware_auto_geometry/include/geometry/intersection.hpp b/common/autoware_auto_geometry/include/geometry/intersection.hpp index 174d18cd2b62c..87dc32b0190d0 100644 --- a/common/autoware_auto_geometry/include/geometry/intersection.hpp +++ b/common/autoware_auto_geometry/include/geometry/intersection.hpp @@ -83,7 +83,7 @@ std::vector get_sorted_face_list(const Iter start, const Iter end) return face_list; } -/// \brief Append points of the polygon `internal` that are contained in the polygon `exernal`. +/// \brief Append points of the polygon `internal` that are contained in the polygon `external`. template < template class Iterable1T, template class Iterable2T, typename PointT> void append_contained_points( @@ -147,6 +147,9 @@ void append_intersection_points( std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)), std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))}; + // cspell: ignore feps + // feps means "Float EPSilon" + // The accumulated floating point error depends on the magnitudes of each end of the // intervals. Hence the upper bound of the absolute magnitude should be taken into account // while computing the epsilon. @@ -274,7 +277,7 @@ std::list convex_polygon_intersection2d( /// \param polygon1 A convex polygon /// \param polygon2 A convex polygon /// \return (Intersection / Union) between two given polygons. -/// \throws std::domain_error If there is any inconsistency on the undderlying geometrical +/// \throws std::domain_error If there is any inconsistency on the underlying geometrical /// computation. template < template class Iterable1T, template class Iterable2T, typename PointT> diff --git a/common/autoware_auto_geometry/include/geometry/interval.hpp b/common/autoware_auto_geometry/include/geometry/interval.hpp index 10cab1a1d7b79..59c26f27cc454 100644 --- a/common/autoware_auto_geometry/include/geometry/interval.hpp +++ b/common/autoware_auto_geometry/include/geometry/interval.hpp @@ -256,8 +256,8 @@ bool Interval::abs_eq(const Interval & i1, const Interval & i2, const T & eps const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2); const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps); - const auto maxs_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); - const auto both_non_empty_equal = both_non_empty && mins_equal && maxs_equal; + const auto maxes_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); + const auto both_non_empty_equal = both_non_empty && mins_equal && maxes_equal; return both_empty || both_non_empty_equal; } @@ -286,7 +286,7 @@ bool Interval::bounds_valid(const Interval & i) { const auto is_ordered = (Interval::min(i) <= Interval::max(i)); - // Check for emptiness expicitly because it requires both bounds to be NaN + // Check for emptiness explicitly because it requires both bounds to be NaN return Interval::empty(i) || is_ordered; } diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp index 227b4db7634cc..e118ec24c7759 100644 --- a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp +++ b/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp @@ -52,7 +52,7 @@ namespace details { /// \brief Internal struct for packing three indices together /// -/// The use of this struct publically is a violation of our coding standards, but I claim it's +/// The use of this struct publicly is a violation of our coding standards, but I claim it's /// fine because (a) it's details, (b) it is literally three unrelated members packaged together. /// This type is needed for conceptual convenience so I don't have massive function parameter /// lists @@ -109,7 +109,9 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp::epsilon(); // lint -e{1938} read only access is fine NOLINT m_max_x -= FEPS; @@ -134,17 +136,17 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp(std::ceil(radius / m_side_length)); + const Index i_radius = static_cast(std::ceil(radius / m_side_length)); // Dumb ternary because potentially unsigned Index type - const Index xmin = (ref.x > iradius) ? (ref.x - iradius) : 0U; - const Index ymin = (ref.y > iradius) ? (ref.y - iradius) : 0U; - const Index zmin = (ref.z > iradius) ? (ref.z - iradius) : 0U; + const Index x_min = (ref.x > i_radius) ? (ref.x - i_radius) : 0U; + const Index y_min = (ref.y > i_radius) ? (ref.y - i_radius) : 0U; + const Index z_min = (ref.z > i_radius) ? (ref.z - i_radius) : 0U; // In 2D mode, m_max_z should be 0, same with ref.z - const Index xmax = std::min(ref.x + iradius, m_max_x_idx); - const Index ymax = std::min(ref.y + iradius, m_max_y_idx); - const Index zmax = std::min(ref.z + iradius, m_max_z_idx); + const Index x_max = std::min(ref.x + i_radius, m_max_x_idx); + const Index y_max = std::min(ref.y + i_radius, m_max_y_idx); + const Index z_max = std::min(ref.z + i_radius, m_max_z_idx); // return bottom-left portion of cube and top-right portion of cube - return {{xmin, ymin, zmin}, {xmax, ymax, zmax}}; + return {{x_min, y_min, z_min}, {x_max, y_max, z_max}}; } /// \brief Get next index within a given range @@ -281,8 +283,8 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); - float32_t dist = static_cast(idist) - 1.0F; + const Index i_dist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); + float32_t dist = static_cast(i_dist) - 1.0F; return std::max(dist, 0.0F); } @@ -302,8 +304,8 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp(max); const float64_t dmin = static_cast(min); const float64_t width = (dmax - dmin) * static_cast(m_side_length_inv); - constexpr float64_t fltmax = static_cast(std::numeric_limits::max()); - if (fltmax <= width) { + constexpr float64_t flt_max = static_cast(std::numeric_limits::max()); + if (flt_max <= width) { throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit"); } return static_cast(width); diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp index dd0ea45198878..225ea099e4e79 100644 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ b/common/autoware_auto_geometry/src/bounding_box.cpp @@ -17,7 +17,9 @@ #include #include #include +// cspell: ignore eigenbox #include +// cspell: ignore lfit #include #include @@ -134,7 +136,7 @@ std::vector GEOMETRY_PUBLIC get_transformed_corners } // namespace details /////////////////////////////////////////////////////////////////////////////// -// precompilation +// pre-compilation using autoware::common::types::PointXYZIF; template BoundingBox minimum_area_bounding_box(std::list & list); template BoundingBox minimum_perimeter_bounding_box(std::list & list); diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp index b1db855f59774..84fa359295613 100644 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp @@ -18,6 +18,7 @@ #define TEST_BOUNDING_BOX_HPP_ #include "geometry/bounding_box/lfit.hpp" +// cspell: ignore lfit #include "geometry/bounding_box/rotating_calipers.hpp" #include @@ -52,6 +53,8 @@ class BoxTest : public ::testing::Test box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points); // apex_test_tools::memory_test::stop(); } + + // cspell: ignore eigenbox template void eigenbox(const IT begin, const IT end) { @@ -128,7 +131,7 @@ using PointTypesBoundingBox = TYPED_TEST_SUITE(BoxTest, PointTypesBoundingBox, ); /// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE -// TODO(c.ho) consider typed and paremterized tests: +// TODO(c.ho) consider typed and parameterized tests: // https://stackoverflow.com/questions/3258230/passing-a-typename-and-string-to-parameterized-test-using-google-test /////////////////////////////////////////// diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp index d9647a212bd61..e7533518d7f49 100644 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp @@ -107,7 +107,7 @@ TYPED_TEST(SanityCheck, Exact) TYPED_TEST(SanityCheck, Interpolation) { const auto x = TypeParam{2}; - // Asssert x is not identically in domain_ + // Assert x is not identically in domain_ ASSERT_TRUE(this->not_in_domain(x)); const auto result = this->table_->lookup(x); this->check(result, TypeParam{3}); diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp index 914f711ef8a7b..6710546fa73e9 100644 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ b/common/autoware_auto_geometry/test/src/test_area.cpp @@ -79,7 +79,7 @@ TYPED_TEST(AreaTest, Triangle) EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h } -// Rectangle is easy to do computationall +// Rectangle is easy to do computational TYPED_TEST(AreaTest, Rectangle) { this->add_point(-5.0, -5.0); diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp index e53c38d8b4335..642e396bdce31 100644 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ b/common/autoware_auto_geometry/test/src/test_common_2d.cpp @@ -188,7 +188,7 @@ TEST(ordered_check, basic) make_points(2.0, 1.0)}; EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - // Colinearity + // Collinearity points_list = { make_points(2.0, 2.0), make_points(4.0, 4.0), diff --git a/common/grid_map_utils/test/test_polygon_iterator.cpp b/common/grid_map_utils/test/test_polygon_iterator.cpp index 71a7e1db8d4f6..f39d080d7cad5 100644 --- a/common/grid_map_utils/test/test_polygon_iterator.cpp +++ b/common/grid_map_utils/test/test_polygon_iterator.cpp @@ -208,7 +208,7 @@ TEST(PolygonIterator, Difference) GridMap map({"layer"}); map.setGeometry(Length(5.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) - // Triangle where the hypothenus is an exact diagonal of the map: difference. + // Triangle where the hypotenuse is an exact diagonal of the map: difference. Polygon polygon; polygon.addVertex(Position(2.5, 2.5)); polygon.addVertex(Position(-2.5, 2.5)); @@ -226,7 +226,7 @@ TEST(PolygonIterator, Difference) } EXPECT_TRUE(diff); - // Triangle where the hypothenus does not cross any cell center: no difference. + // Triangle where the hypotenuse does not cross any cell center: no difference. polygon.removeVertices(); polygon.addVertex(Position(2.5, 2.1)); polygon.addVertex(Position(-2.5, 2.5)); diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 848d65dfae575..61c60df7a8984 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -59,7 +59,7 @@ std::array, 4> getBaseValues( // calculate base_keys, base_values if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) { - throw std::logic_error("The numbef of unique points is not enough."); + throw std::logic_error("The number of unique points is not enough."); } const std::vector base_s = calcEuclidDist(base_x, base_y); diff --git a/common/signal_processing/documentation/ButterworthFilter.md b/common/signal_processing/documentation/ButterworthFilter.md index e29ce1f4cfb69..2eac82da83bac 100644 --- a/common/signal_processing/documentation/ButterworthFilter.md +++ b/common/signal_processing/documentation/ButterworthFilter.md @@ -118,6 +118,8 @@ At this step, we define a boolean variable whether to use the pre-warping option **References:** + + 1. Manolakis, Dimitris G., and Vinay K. Ingle. Applied digital signal processing: theory and practice. Cambridge University Press, 2011. diff --git a/common/signal_processing/src/butterworth.cpp b/common/signal_processing/src/butterworth.cpp index 3b4263461e9f6..88a8b6c194601 100644 --- a/common/signal_processing/src/butterworth.cpp +++ b/common/signal_processing/src/butterworth.cpp @@ -190,6 +190,7 @@ void ButterworthFilter::printContinuousTimeTF() const RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[%s]", tf_text.c_str()); } +// cspell: ignore dend /** * @brief This method assumes the continuous time transfer function of filter has already been * computed and stored in the object and uses the bilinear transformation to obtain the discrete diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index 50028f51332e6..0b9ea27004cb1 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -39,6 +39,7 @@ TrtCommon::TrtCommon( { for (const auto & plugin_path : plugin_paths) { int32_t flags{RTLD_LAZY}; +// cspell: ignore asan #if ENABLE_ASAN // https://github.com/google/sanitizers/issues/89 // asan doesn't handle module unloading correctly and there are no plans on doing diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 123b5976271cf..1fa87c2caf3a9 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -11,7 +11,9 @@ Based on the various input from planning, control, and vehicle, it publishes the All results in `ErrorStamped` message are calculated in Frenet Frame of curve. Errors and velocity errors are calculated by using paper below. -`Werling, Moritz & Groell, Lutz & Bretthauer, Georg. (2010). Invariant Trajectory Tracking With a Full-Size Autonomous Road Vehicle. Robotics, IEEE Transactions on. 26. 758 - 765. 10.1109/TRO.2010.2052325.` + + +`Werling, Moritz & Groell, Lutz & Bretthauer, Georg. (2010). Invariant Trajectory Tracking With a Full-Size Autonomous Road Vehicle. IEEE Transactions on Robotics. 26. 758 - 765. 10.1109/TRO.2010.2052325.` If you are interested in calculations, you can see the error and error velocity calculations in section `C. Asymptotical Trajectory Tracking With Orientation Control`. diff --git a/control/control_performance_analysis/config/controller_monitor.xml b/control/control_performance_analysis/config/controller_monitor.xml index e1a3bbc2bce80..c5189ccec4110 100644 --- a/control/control_performance_analysis/config/controller_monitor.xml +++ b/control/control_performance_analysis/config/controller_monitor.xml @@ -401,7 +401,7 @@ - + diff --git a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp index ce11e10700487..423c71fb286fc 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -40,11 +40,11 @@ double curvatureFromThreePoints( { double area = triangleArea(a, b, c); - double amag = std::hypot(a[0] - b[0], a[1] - b[1]); // magnitude of triangle edges - double bmag = std::hypot(b[0] - c[0], b[1] - c[1]); - double cmag = std::hypot(c[0] - a[0], c[1] - a[1]); + double a_mag = std::hypot(a[0] - b[0], a[1] - b[1]); // magnitude of triangle edges + double b_mag = std::hypot(b[0] - c[0], b[1] - c[1]); + double c_mag = std::hypot(c[0] - a[0], c[1] - a[1]); - double curvature = 4 * area / std::max(amag * bmag * cmag, 1e-4); + double curvature = 4 * area / std::max(a_mag * b_mag * c_mag, 1e-4); return curvature; } diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 7596f753c5edc..efd6480e214e1 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -31,6 +31,8 @@ Different vehicle models are implemented: For the optimization, a Quadratic Programming (QP) solver is used and two options are currently implemented: + + - unconstraint_fast : use least square method to solve unconstraint QP with eigen. - [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) algorithm (for more details see the related papers at the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index e9a8e50318757..2e7837dc6bbe1 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -58,7 +58,7 @@ class MPCTrajectory */ size_t size() const; /** - * @return true if the compensents sizes are all 0 or are inconsistent + * @return true if the components sizes are all 0 or are inconsistent */ inline bool empty() const { return size() == 0; } diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index b7d35eca6cbe5..d9fce24f4185f 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -84,7 +84,7 @@ bool convertToAutowareTrajectory( /** * @brief calculate the arc length at each point of the given trajectory * @param [in] trajectory trajectory for which to calculate the arc length - * @param [out] arclength the cummulative arc length at each point of the trajectory + * @param [out] arclength the cumulative arc length at each point of the trajectory */ void calcMPCTrajectoryArclength(const MPCTrajectory & trajectory, std::vector * arclength); /** diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index cb4f92c0c6774..a189ecc2ea7c8 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -41,7 +41,7 @@ class QPSolverInterface * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a * @param [out] u optimal variable vector - * @return ture if the problem was solved + * @return true if the problem was solved */ virtual bool solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, diff --git a/control/mpc_lateral_controller/test/test_interpolate.cpp b/control/mpc_lateral_controller/test/test_interpolate.cpp index 97b2d28205ee4..d9be96b0ef419 100644 --- a/control/mpc_lateral_controller/test/test_interpolate.cpp +++ b/control/mpc_lateral_controller/test/test_interpolate.cpp @@ -78,7 +78,7 @@ TEST(TestInterpolate, Failure) ASSERT_FALSE( linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 3.5}, target_values)); - // Missmatched inputs + // Mismatched inputs ASSERT_FALSE(linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0}, {1.0, 1.5}, target_values)); ASSERT_FALSE(linearInterpolate({1.0, 2.0, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 1.5}, target_values)); diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp index 5441eaa200ac2..a5b1e17ed983f 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp +++ b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp @@ -79,6 +79,7 @@ bool isDirectionForward( bool isDirectionForward( const geometry_msgs::msg::Pose & prev, const geometry_msgs::msg::Point & next); +// cspell: ignore pointinpoly // refer from apache's pointinpoly in http://www.visibone.com/inpoly/ template bool isInPolygon(const std::vector & polygon, const T & point) diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp index 2c2fe2199f007..0f9c0d57bb5cd 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp @@ -36,8 +36,8 @@ class LongitudinalControllerBase virtual bool isReady(const InputData & input_data) = 0; virtual LongitudinalOutput run(InputData const & input_data) = 0; void sync(LateralSyncData const & lateral_sync_data); - // NOTE: This reset function should be called when the trajectory is replaned by changing ego pose - // or goal pose. + // NOTE: This reset function should be called when the trajectory is replanned by changing ego + // pose or goal pose. void reset(); protected: diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index f77e9b263a389..bdcc7ac486cd0 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -164,7 +164,7 @@ Increasing the number will improve the smoothness of the estimation, but may hav - `proc_stddev_vx_c` : set to maximum linear acceleration - `proc_stddev_wz_c` : set to maximum angular acceleration -- `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yawrate. A large value means the change in yaw does not correlate to the estimated yawrate. If this is set to 0, it means the change in estimated yaw is equal to yawrate. Usually, this should be set to 0. +- `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yaw rate. A large value means the change in yaw does not correlate to the estimated yaw rate. If this is set to 0, it means the change in estimated yaw is equal to yaw rate. Usually, this should be set to 0. - `proc_stddev_yaw_bias_c` : This parameter is the standard deviation for the rate of change in yaw bias. In most cases, yaw bias is constant, so it can be very small, but must be non-zero. ## Kalman Filter Model diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 7c63785aebcd2..14d3c9e8cca84 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -241,6 +241,8 @@ This is a function that using de-grounded LiDAR scan estimate scan matching scor ### Parameters + + | Name | Type | Description | | ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | | `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 56c5baa347aaa..a5d8142b6616e 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -73,6 +73,7 @@ # Radius of input LiDAR range (used for diagnostics of dynamic map loading) lidar_radius: 100.0 + # cspell: ignore degrounded # A flag for using scan matching score based on de-grounded LiDAR scan estimate_scores_for_degrounded_scan: false diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 1bd7a509a3a7b..7c9dab4c8103b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -203,6 +203,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr pose_init_module_; std::unique_ptr map_update_module_; + // cspell: ignore degrounded bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; }; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a9c37d7843a64..32a286a55ef37 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -77,6 +77,7 @@ bool validate_local_optimal_solution_oscillation( return false; } +// cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), diff --git a/map/map_tf_generator/test/test_uniform_random.cpp b/map/map_tf_generator/test/test_uniform_random.cpp index 76ee174a34871..455edc2d5dfd2 100644 --- a/map/map_tf_generator/test/test_uniform_random.cpp +++ b/map/map_tf_generator/test/test_uniform_random.cpp @@ -29,7 +29,7 @@ TEST(UniformRandom, UniformRandom) } // checks if the returned values are in range of [min, max) - // note that the minimun range is always zero and the max value is exclusive + // note that the minimum range is always zero and the max value is exclusive { const size_t min_inclusive = 0; const size_t max_exclusive = 4; diff --git a/perception/elevation_map_loader/README.md b/perception/elevation_map_loader/README.md index 7bfdf5729127c..e9288c2f474a1 100644 --- a/perception/elevation_map_loader/README.md +++ b/perception/elevation_map_loader/README.md @@ -103,7 +103,7 @@ See f | Name | Type | Description | Default value | | :--------------------------------------------------------- | :---- | :----------------------------------------------------------------------------- | :------------ | | pcl_grid_map_extraction/outlier_removal/is_remove_outliers | float | Whether to perform statistical outlier removal. | false | -| pcl_grid_map_extraction/outlier_removal/mean_K | float | Number of neighbours to analyze for estimating statistics of a point. | 10 | +| pcl_grid_map_extraction/outlier_removal/mean_K | float | Number of neighbors to analyze for estimating statistics of a point. | 10 | | pcl_grid_map_extraction/outlier_removal/stddev_threshold | float | Number of standard deviations under which points are considered to be inliers. | 1.0 | #### Subsampling parameters diff --git a/perception/ground_segmentation/include/ground_segmentation/gencolors.hpp b/perception/ground_segmentation/include/ground_segmentation/gencolors.hpp index 0adefe0984347..07637b1514b09 100644 --- a/perception/ground_segmentation/include/ground_segmentation/gencolors.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/gencolors.hpp @@ -97,6 +97,8 @@ inline static void downsamplePoints(const Mat & src, Mat & dst, size_t count) dst.at>(0) = src.at>(maxLoc.x); dst.at>(1) = src.at>(maxLoc.y); + // ignore cspell error due to the source from OpenCV + // cspell: ignore actived Dists randu Mat activedDists(0, dists.cols, dists.type()); Mat candidatePointsMask(1, dists.cols, CV_8UC1, Scalar(255)); activedDists.push_back(dists.row(maxLoc.y)); diff --git a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp b/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp index 4b11db83ad668..508ce238b51f1 100644 --- a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp +++ b/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp @@ -64,7 +64,7 @@ class HeatmapVisualizerNode : public rclcpp::Node float map_length_; float map_resolution_; bool use_confidence_; - std::vector class_names_{"UNKNWON", "CAR", "TRUCK", "BUS", + std::vector class_names_{"UNKNOWN", "CAR", "TRUCK", "BUS", "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; bool rename_car_to_truck_and_bus_; diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 65d2fe5cc849d..700724a817ceb 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - point_feature_size: 7 # x, y, z, timelag and car, pedestrian, bicycle + point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle max_voxel_size: 40000 point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] voxel_size: [0.32, 0.32, 8.0] diff --git a/perception/image_projection_based_fusion/docs/pointpainting-fusion.md b/perception/image_projection_based_fusion/docs/pointpainting-fusion.md index 0d5c9a280cca5..b15a7c5fef9de 100644 --- a/perception/image_projection_based_fusion/docs/pointpainting-fusion.md +++ b/perception/image_projection_based_fusion/docs/pointpainting-fusion.md @@ -72,8 +72,12 @@ Example: ## References/External links + + [1] Vora, Sourabh, et al. "PointPainting: Sequential fusion for 3d object detection." Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2020. + + [2] CVPR'20 Workshop on Scalability in Autonomous Driving] Waymo Open Dataset Challenge: Ding, Zhuangzhuang, et al. "1st Place Solution for Waymo Open Dataset Challenge--3D Detection and Domain Adaptation." arXiv preprint arXiv:2006.15505 (2020). diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index bf704b0a9311d..4b33783653708 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -40,6 +40,8 @@ #include #include +// cspell: ignore minx, maxx, miny, maxy, minz, maxz + namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; @@ -109,7 +111,7 @@ class FusionNode : public rclcpp::Node // cache for fusion std::vector is_fused_; - std::pair sub_stdpair_; + std::pair sub_std_pair_; std::vector> roi_stdmap_; std::mutex mutex_; @@ -119,6 +121,7 @@ class FusionNode : public rclcpp::Node // debugger std::shared_ptr debugger_; virtual bool out_of_scope(const ObjType & obj) = 0; + // cspell: ignore minx, maxx, miny, maxy, minz, maxz float filter_scope_minx_; float filter_scope_maxx_; float filter_scope_miny_; diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 2848a16c33af1..2bf9af331ca45 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -26,6 +26,7 @@ + diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 72a36b0afcc6a..656adaac252fc 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -115,7 +115,8 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } - + // cspell: ignore minx, maxx, miny, maxy, minz, maxz + // FIXME: use min_x instead of minx filter_scope_minx_ = declare_parameter("filter_scope_minx", -100); filter_scope_maxx_ = declare_parameter("filter_scope_maxx", 100); filter_scope_miny_ = declare_parameter("filter_scope_miny", -100); @@ -160,7 +161,7 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ int64_t timestamp_nsec = (*output_msg).header.stamp.sec * (int64_t)1e9 + (*output_msg).header.stamp.nanosec; - // if matching rois exist, fuseonsingle + // if matching rois exist, fuseOnSingle for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i); @@ -173,13 +174,13 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ std::list outdate_stamps; for (const auto & [k, v] : roi_stdmap_.at(roi_i)) { - int64_t newstamp = timestamp_nsec + input_offset_ms_.at(roi_i) * (int64_t)1e6; - int64_t interval = abs(int64_t(k) - newstamp); + int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * (int64_t)1e6; + int64_t interval = abs(int64_t(k) - new_stamp); if (interval <= min_interval && interval <= match_threshold_ms_ * (int64_t)1e6) { min_interval = interval; matched_stamp = k; - } else if (int64_t(k) < newstamp && interval > match_threshold_ms_ * (int64_t)1e6) { + } else if (int64_t(k) < new_stamp && interval > match_threshold_ms_ * (int64_t)1e6) { outdate_stamps.push_back(int64_t(k)); } } @@ -189,7 +190,7 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ (roi_stdmap_.at(roi_i)).erase(stamp); } - // fuseonSingle + // fuseOnSingle if (matched_stamp != -1) { if (debugger_) { debugger_->clear(); @@ -232,10 +233,10 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ "debug/processing_time_ms", processing_time_ms); } } else { - if (sub_stdpair_.second != nullptr) { + if (sub_std_pair_.second != nullptr) { timer_->cancel(); - postprocess(*(sub_stdpair_.second)); - publish(*(sub_stdpair_.second)); + postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); std::fill(is_fused_.begin(), is_fused_.end(), false); // add processing time for debug @@ -249,8 +250,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } } - sub_stdpair_.first = int64_t(timestamp_nsec); - sub_stdpair_.second = output_msg; + sub_std_pair_.first = int64_t(timestamp_nsec); + sub_std_pair_.second = output_msg; } } @@ -262,9 +263,9 @@ void FusionNode::roiCallback( (*input_roi_msg).header.stamp.sec * (int64_t)1e9 + (*input_roi_msg).header.stamp.nanosec; // if cached Msg exist, try to match - if (sub_stdpair_.second != nullptr) { - int64_t newstamp = sub_stdpair_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6; - int64_t interval = abs(timestamp_nsec - newstamp); + if (sub_std_pair_.second != nullptr) { + int64_t new_stamp = sub_std_pair_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6; + int64_t interval = abs(timestamp_nsec - new_stamp); if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { @@ -277,12 +278,12 @@ void FusionNode::roiCallback( } fuseOnSingleImage( - *(sub_stdpair_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), - *(sub_stdpair_.second)); + *(sub_std_pair_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), + *(sub_std_pair_.second)); is_fused_.at(roi_i) = true; if (debug_publisher_) { - double timestamp_interval_ms = (timestamp_nsec - sub_stdpair_.first) / 1e6; + double timestamp_interval_ms = (timestamp_nsec - sub_std_pair_.first) / 1e6; debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( @@ -292,10 +293,10 @@ void FusionNode::roiCallback( if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - postprocess(*(sub_stdpair_.second)); - publish(*(sub_stdpair_.second)); + postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); std::fill(is_fused_.begin(), is_fused_.end(), false); - sub_stdpair_.second = nullptr; + sub_std_pair_.second = nullptr; // add processing time for debug if (debug_publisher_) { @@ -324,12 +325,12 @@ void FusionNode::timer_callback() timer_->cancel(); if (mutex_.try_lock()) { // timeout, postprocess cached msg - if (sub_stdpair_.second != nullptr) { - postprocess(*(sub_stdpair_.second)); - publish(*(sub_stdpair_.second)); + if (sub_std_pair_.second != nullptr) { + postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); } std::fill(is_fused_.begin(), is_fused_.end(), false); - sub_stdpair_.second = nullptr; + sub_std_pair_.second = nullptr; // add processing time for debug if (debug_publisher_) { diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index 678cf8a5febac..de590c5d91984 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -39,6 +39,7 @@ const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32; // the same as max_point_in_vox const std::size_t WARPS_PER_BLOCK = 4; const std::size_t ENCODER_IN_FEATURE_SIZE = 12; // same as encoder_in_feature_size_ in config.hpp +// cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) { if (a == 0) { diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index 69e9bfd912745..d195f7870fc71 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -46,7 +46,7 @@ std::size_t VoxelGenerator::pointsToVoxels( auto pc_msg = pc_cache_iter->pointcloud_msg; auto affine_past2current = pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; - float timelag = static_cast( + float time_lag = static_cast( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); for (sensor_msgs::PointCloud2ConstIterator x_iter(pc_msg, "x"), y_iter(pc_msg, "y"), @@ -59,7 +59,7 @@ std::size_t VoxelGenerator::pointsToVoxels( point[0] = point_current.x(); point[1] = point_current.y(); point[2] = point_current.z(); - point[3] = timelag; + point[3] = time_lag; point[4] = *car_iter; point[5] = *ped_iter; point[6] = *bic_iter; diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 717f21da21fc3..e15d47a01521c 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -28,6 +28,8 @@ #include #endif +// cspell: ignore minx, maxx, miny, maxy, minz, maxz + namespace image_projection_based_fusion { diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 387dd08bc3c55..3781714d3cf71 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -17,6 +17,8 @@ #include #include +// cspell: ignore minx, maxx, miny, maxy, minz, maxz + namespace image_projection_based_fusion { diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index a58a1121fc4a6..363184ecacfa9 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -78,7 +78,7 @@ class CenterPointConfig // input params std::size_t class_size_{3}; const std::size_t point_dim_size_{3}; // x, y and z - std::size_t point_feature_size_{4}; // x, y, z and timelag + std::size_t point_feature_size_{4}; // x, y, z and time-lag std::size_t max_point_in_voxel_size_{32}; std::size_t max_voxel_size_{40000}; float range_min_x_{-89.6f}; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp index 36849479a87ea..84462bc9657ac 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp @@ -35,6 +35,7 @@ struct Box3D float vel_y; }; +// cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b); } // namespace centerpoint diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md index c78ac8d235b5d..69ef70be265ec 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md @@ -40,7 +40,7 @@ source install/setup.bash ros2 bag play /YOUR/ROSBAG/PATH/ --clock 100 ``` -Don't forget to add `clock` inorder to sync between two rviz display. +Don't forget to add `clock` in order to sync between two rviz display. You can also use the sample rosbag provided by autoware [here](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/rosbag-replay-simulation/). diff --git a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu index cba72c2c37841..da2f0e2f57c15 100644 --- a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu +++ b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu @@ -48,6 +48,7 @@ __global__ void scatterFeatures_kernel( feature; } +// cspell: ignore divup cudaError_t scatterFeatures_launch( const float * pillar_features, const int * coords, const std::size_t num_pillars, const std::size_t max_voxel_size, const std::size_t encoder_out_feature_size, diff --git a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu index 902c27fe8add1..398e75a55c44b 100644 --- a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu @@ -41,6 +41,7 @@ __device__ inline float dist2dPow(const Box3D * a, const Box3D * b) return powf(a->x - b->x, 2) + powf(a->y - b->y, 2); } +// cspell: ignore divup __global__ void circleNMS_Kernel( const Box3D * boxes, const std::size_t num_boxes3d, const std::size_t col_blocks, const float dist2d_pow_threshold, std::uint64_t * mask) diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 8942bdd33ceb5..765f678784574 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -114,6 +114,7 @@ PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(con config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); } +// cspell: ignore divup cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( const float * out_heatmap, const float * out_offset, const float * out_z, const float * out_dim, const float * out_rot, const float * out_vel, std::vector & det_boxes3d, @@ -130,7 +131,7 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), thrust::raw_pointer_cast(boxes3d_d_.data())); - // suppress by socre + // suppress by score const auto num_det_boxes3d = thrust::count_if( thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), is_score_greater(config_.score_threshold_)); diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index b647a75504180..7b59757311ff2 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -141,6 +141,7 @@ __global__ void generateFeatures_kernel( } } +// cspell: ignore divup cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index 8f9be13d5cd45..7f4e4a849211c 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -71,7 +71,7 @@ std::size_t VoxelGenerator::pointsToVoxels( pd_ptr_->pointcloud_cache_size() > 1 ? pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world : Eigen::Affine3f::Identity(); - float timelag = static_cast( + float time_lag = static_cast( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); for (sensor_msgs::PointCloud2ConstIterator x_iter(pc_msg, "x"), y_iter(pc_msg, "y"), @@ -83,7 +83,7 @@ std::size_t VoxelGenerator::pointsToVoxels( point[0] = point_current.x(); point[1] = point_current.y(); point[2] = point_current.z(); - point[3] = timelag; + point[3] = time_lag; out_of_range = false; for (std::size_t di = 0; di < config_.point_dim_size_; di++) { diff --git a/perception/lidar_centerpoint/lib/utils.cpp b/perception/lidar_centerpoint/lib/utils.cpp index 75508eb32af20..b6e0a54ab6de9 100644 --- a/perception/lidar_centerpoint/lib/utils.cpp +++ b/perception/lidar_centerpoint/lib/utils.cpp @@ -18,6 +18,7 @@ namespace centerpoint { +// cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) { if (a == 0) { diff --git a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/utils.hpp b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/utils.hpp index 7ff2c6eb612b0..259deef53f189 100644 --- a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/utils.hpp +++ b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/utils.hpp @@ -40,6 +40,7 @@ struct Box3D float vel_y; }; +// cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b); } // namespace lidar_centerpoint_tvm diff --git a/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp b/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp index 93563555e03d5..405223c800eba 100644 --- a/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp +++ b/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp @@ -114,6 +114,7 @@ void generateBoxes3D_worker( } } +// cspell: ignore divup void generateDetectedBoxes3D( const std::vector & out_heatmap, const std::vector & out_offset, const std::vector & out_z, const std::vector & out_dim, @@ -135,7 +136,7 @@ void generateDetectedBoxes3D( threadPool[idx].join(); } - // suppress by socre + // suppress by score const auto num_det_boxes3d = std::count_if(boxes3d.begin(), boxes3d.end(), is_score_greater(config.score_threshold_)); if (num_det_boxes3d == 0) { @@ -150,17 +151,17 @@ void generateDetectedBoxes3D( // sort by score std::sort(det_boxes3d_nonms.begin(), det_boxes3d_nonms.end(), score_greater()); - // supress by NMS + // suppress by NMS std::vector final_keep_mask(num_det_boxes3d); const auto num_final_det_boxes3d = circleNMS(det_boxes3d_nonms, config.circle_nms_dist_threshold_, final_keep_mask); det_boxes3d.resize(num_final_det_boxes3d); - std::size_t boxid = 0; + std::size_t box_id = 0; for (std::size_t idx = 0; idx < final_keep_mask.size(); idx++) { if (final_keep_mask[idx]) { - det_boxes3d[boxid] = det_boxes3d_nonms[idx]; - boxid++; + det_boxes3d[box_id] = det_boxes3d_nonms[idx]; + box_id++; } } // std::copy_if(det_boxes3d_nonms.begin(), det_boxes3d_nonms.end(), final_keep_mask.begin(), diff --git a/perception/lidar_centerpoint_tvm/lib/preprocess/generate_features.cpp b/perception/lidar_centerpoint_tvm/lib/preprocess/generate_features.cpp index c095ff7311eb6..9b98adc2def4e 100644 --- a/perception/lidar_centerpoint_tvm/lib/preprocess/generate_features.cpp +++ b/perception/lidar_centerpoint_tvm/lib/preprocess/generate_features.cpp @@ -52,7 +52,7 @@ void generateFeatures_worker( pillar_idx * config.max_point_in_voxel_size_ * config.point_feature_size_ + i * config.point_feature_size_; for (std::size_t j = 0; j < config.point_feature_size_; j++) { - // point (x, y, z, instensity) + // point (x, y, z, intensity) if (i < points_count && j < 3) points_sum[j] += voxel_features[point_idx + j]; } } @@ -104,6 +104,7 @@ void generateFeatures_worker( } } +// cspell: ignore divup void generateFeatures( const std::vector & voxel_features, const std::vector & voxel_num_points, const std::vector & coords, const std::size_t num_voxels, diff --git a/perception/lidar_centerpoint_tvm/lib/utils.cpp b/perception/lidar_centerpoint_tvm/lib/utils.cpp index 146c47f05570c..caf9cb84fa1c7 100644 --- a/perception/lidar_centerpoint_tvm/lib/utils.cpp +++ b/perception/lidar_centerpoint_tvm/lib/utils.cpp @@ -23,6 +23,7 @@ namespace perception namespace lidar_centerpoint_tvm { +// cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) { if (a == 0) { diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 20d76b3841f6d..043103e9c39d7 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -42,7 +42,7 @@ Search one or more lanelets satisfying the following conditions for each target - Create a reference path for the object from the associated lanelet. - Predict Object Maneuver - Generate predicted paths for the object. - - The probability is assigned to each maneuver of `Lane Follow`, `Left Lane Change`, and `Right Lane Chagne` based on the object history and the reference path obtained in the first step. + - The probability is assigned to each maneuver of `Lane Follow`, `Left Lane Change`, and `Right Lane Change` based on the object history and the reference path obtained in the first step. - The following information is used to determine the maneuver. - The distance between the current center of gravity of the object and the left and right boundaries of the lane - The lateral velocity (distance moved to the lateral direction in `t` seconds) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 7544a204d3e8e..bc97c9ba92d7e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -330,7 +330,7 @@ inline void convertConvexHullToBoundingBox( const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; const Eigen::Vector2d new_center = center + Rinv.transpose() * new_local_center; - // set output paramters + // set output parameters output_object = input_object; output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg index f6714b4904813..b253222c7b311 100644 --- a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg @@ -187,7 +187,7 @@
- Delete obejct + Delete object
@@ -195,7 +195,7 @@
- Delete obejct + Delete object diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md index aaf51198e4429..390d5a2198cb3 100644 --- a/perception/tensorrt_yolox/README.md +++ b/perception/tensorrt_yolox/README.md @@ -8,6 +8,8 @@ This package detects target objects e.g., cars, trucks, bicycles, and pedestrian ### Cite + + Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Series in 2021", arXiv preprint arXiv:2107.08430, 2021 [[ref](https://arxiv.org/abs/2107.08430)] ## Inputs / Outputs diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp index 7f0f64dc89f67..8ee0c8e1ed59e 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp @@ -74,7 +74,7 @@ class TrtYoloX void generateYoloxProposals( std::vector grid_strides, float * feat_blob, float prob_threshold, ObjectArray & objects) const; - void qsortDescentInplace(ObjectArray & faceobjects, int left, int right) const; + void qsortDescentInplace(ObjectArray & face_objects, int left, int right) const; inline void qsortDescentInplace(ObjectArray & objects) const { if (objects.empty()) { @@ -89,8 +89,9 @@ class TrtYoloX cv::Rect_ inter = a_rect & b_rect; return inter.area(); } + // cspell: ignore Bboxes void nmsSortedBboxes( - const ObjectArray & faceobjects, std::vector & picked, float nms_threshold) const; + const ObjectArray & face_objects, std::vector & picked, float nms_threshold) const; std::unique_ptr trt_common_; diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index 191a3832d028d..e940679c9c58d 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -61,7 +61,7 @@ TrtYoloX::TrtYoloX( break; default: std::stringstream s; - s << "\"" << model_path << "\" is unsuppoerted format"; + s << "\"" << model_path << "\" is unsupported format"; std::runtime_error{s.str()}; } @@ -227,6 +227,7 @@ void TrtYoloX::decodeOutputs( qsortDescentInplace(proposals); std::vector picked; + // cspell: ignore Bboxes nmsSortedBboxes(proposals, picked, nms_threshold_); int count = static_cast(picked.size()); @@ -310,24 +311,24 @@ void TrtYoloX::generateYoloxProposals( } // point anchor loop } -void TrtYoloX::qsortDescentInplace(ObjectArray & faceobjects, int left, int right) const +void TrtYoloX::qsortDescentInplace(ObjectArray & face_objects, int left, int right) const { int i = left; int j = right; - float p = faceobjects[(left + right) / 2].score; + float p = face_objects[(left + right) / 2].score; while (i <= j) { - while (faceobjects[i].score > p) { + while (face_objects[i].score > p) { i++; } - while (faceobjects[j].score < p) { + while (face_objects[j].score < p) { j--; } if (i <= j) { // swap - std::swap(faceobjects[i], faceobjects[j]); + std::swap(face_objects[i], face_objects[j]); i++; j--; @@ -339,38 +340,38 @@ void TrtYoloX::qsortDescentInplace(ObjectArray & faceobjects, int left, int righ #pragma omp section { if (left < j) { - qsortDescentInplace(faceobjects, left, j); + qsortDescentInplace(face_objects, left, j); } } #pragma omp section { if (i < right) { - qsortDescentInplace(faceobjects, i, right); + qsortDescentInplace(face_objects, i, right); } } } } void TrtYoloX::nmsSortedBboxes( - const ObjectArray & faceobjects, std::vector & picked, float nms_threshold) const + const ObjectArray & face_objects, std::vector & picked, float nms_threshold) const { picked.clear(); - const int n = faceobjects.size(); + const int n = face_objects.size(); std::vector areas(n); for (int i = 0; i < n; i++) { cv::Rect rect( - faceobjects[i].x_offset, faceobjects[i].y_offset, faceobjects[i].width, - faceobjects[i].height); + face_objects[i].x_offset, face_objects[i].y_offset, face_objects[i].width, + face_objects[i].height); areas[i] = rect.area(); } for (int i = 0; i < n; i++) { - const Object & a = faceobjects[i]; + const Object & a = face_objects[i]; int keep = 1; for (int j = 0; j < static_cast(picked.size()); j++) { - const Object & b = faceobjects[picked[j]]; + const Object & b = face_objects[picked[j]]; // intersection over union float inter_area = intersectionArea(a, b); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 527bb50c5dabf..848aaf8c8d38c 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2568,7 +2568,7 @@ ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep // extending during avoidance module -// TODO(murooka) freespace during turning in intersection where there is no neighbour lanes +// TODO(murooka) freespace during turning in intersection where there is no neighbor lanes // NOTE: Assume that there is no situation where there is an object in the middle lane of more than // two lanes since which way to avoid is not obvious void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const @@ -2642,7 +2642,7 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const const auto next_lanes_for_left = get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); - // 2.2 look for neighbour lane recursively, where end line of the lane is connected to end line + // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line // of the original lane const auto update_drivable_lanes = [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { diff --git a/planning/behavior_velocity_planner/crosswalk-design.md b/planning/behavior_velocity_planner/crosswalk-design.md index 4897737b98c0c..19fa4a5ec5da1 100644 --- a/planning/behavior_velocity_planner/crosswalk-design.md +++ b/planning/behavior_velocity_planner/crosswalk-design.md @@ -175,7 +175,7 @@ This module uses the larger value of estimated object velocity and `min_object_v ```plantuml start -if (Pedestrain's traffic light signal is **RED**?) then (yes) +if (Pedestrian's traffic light signal is **RED**?) then (yes) else (no) if (There are objects around the crosswalk?) then (yes) :calculate TTC & TTV; diff --git a/planning/behavior_velocity_planner/docs/crosswalk/limitation.svg b/planning/behavior_velocity_planner/docs/crosswalk/limitation.svg index b959328e63330..3e507c0669a2b 100644 --- a/planning/behavior_velocity_planner/docs/crosswalk/limitation.svg +++ b/planning/behavior_velocity_planner/docs/crosswalk/limitation.svg @@ -142,12 +142,12 @@
- The pedestrain has no intention to cross via this route. + The pedestrian has no intention to cross via this route.
- The pedestrain has no intent... + The pedestrian has no intent... diff --git a/planning/freespace_planner/README.md b/planning/freespace_planner/README.md index df834305519eb..b40f2681dce51 100644 --- a/planning/freespace_planner/README.md +++ b/planning/freespace_planner/README.md @@ -79,12 +79,14 @@ None #### RRT\* search parameters + + | Parameter | Type | Description | | ----------------------- | ------ | ----------------------------------------------------------------------------- | | `max planning time` | double | maximum planning time [msec] (used only when `enable_update` is set `true`) | | `enable_update` | bool | whether update after feasible solution found until `max_planning time` elapse | -| `use_informed_sampling` | bool | Use informed RRT\* (of Gammmell et al.) | -| `neighbour_radius` | double | neighbour radius of RRT\* algorithm | +| `use_informed_sampling` | bool | Use informed RRT\* (of Gammell et al.) | +| `neighbor_radius` | double | neighbor radius of RRT\* algorithm | | `margin` | double | safety margin ensured in path's collision checking in RRT\* algorithm | ### Flowchart diff --git a/planning/freespace_planning_algorithms/README.md b/planning/freespace_planning_algorithms/README.md index ef0c2a3aca384..0084f4c296547 100644 --- a/planning/freespace_planning_algorithms/README.md +++ b/planning/freespace_planning_algorithms/README.md @@ -10,10 +10,12 @@ This package is for development of path planning algorithms in free space. Please see [rrtstar.md](rrtstar.md) for a note on the implementation for informed-RRT\*. + + NOTE: As for RRT\*, one can choose whether update after feasible solution found in RRT\*. If not doing so, the algorithm is the almost (but exactly because of rewiring procedure) same as vanilla RRT. If you choose update, then you have option if the sampling after feasible solution found is "informed". -If set true, then the algorithm is equivalent to `informed RRT\* of Gammmell et al. 2014`. +If set true, then the algorithm is equivalent to `informed RRT\* of Gammell et al. 2014`. ## Algorithm selection diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp index 6b3b967760e45..eb091b9066bff 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp @@ -30,7 +30,7 @@ struct RRTStarParam bool enable_update; // update solution even after feasible solution found with given time budget bool use_informed_sampling; // use informed sampling (informed rrtstar) double max_planning_time; // if enable_update is true, update is done before time elapsed [msec] - double neighbour_radius; // neighbore radius [m] + double neighbor_radius; // neighbor radius [m] double margin; // [m] }; @@ -61,7 +61,7 @@ class RRTStar : public AbstractPlanningAlgorithm bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const override; private: - void setRRTPath(const std::vector & waypints); + void setRRTPath(const std::vector & waypoints); // algorithm specific param const RRTStarParam rrtstar_param_; diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar_core.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar_core.hpp index b0ad43dbf3494..dcaeba804944c 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar_core.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar_core.hpp @@ -38,6 +38,7 @@ using Pose = freespace_planning_algorithms::ReedsSheppStateSpace::StateXYT; using ReedsSheppStateSpace = freespace_planning_algorithms::ReedsSheppStateSpace; const double inf = std::numeric_limits::infinity(); +// cspell: ignore rsspace class CSpace { public: @@ -59,7 +60,7 @@ class CSpace // Note that parent-to-child and child-to-parent reeds-shepp paths are sometimes // different, because there may two reeds-shepp paths with an exact same cost given two poses. // For example, say you want to compute a reeds-shepp path from [0, 0, 0] to [0, 1, 0], where - // each element indicate x, y, and yaw. In such a case, you will have symetric two reeds-sheep + // each element indicate x, y, and yaw. In such a case, you will have symmetric two reeds-sheep // paths, one of which starts moving with forward motion and // the other of which with backward motion. // So, due to floating point level difference, the resulting reeds-shepp path can be different. @@ -137,15 +138,15 @@ class RRTStar private: NodeConstSharedPtr findNearestNode(const Pose & x_rand) const; - std::vector findNeighboreNodes(const Pose & pose) const; + std::vector findNeighborNodes(const Pose & pose) const; NodeSharedPtr addNewNode(const Pose & pose, NodeSharedPtr node_parent); NodeConstSharedPtr getBestParentNode( const Pose & pose_new, const NodeConstSharedPtr & node_nearest, - const std::vector & neighbore_nodes) const; + const std::vector & neighbor_nodes) const; void reconnect(const NodeSharedPtr & node_new, const NodeSharedPtr & node_reconnect); NodeConstSharedPtr getReconnectTargeNode( const NodeConstSharedPtr node_new, - const std::vector & neighbore_nodes) const; + const std::vector & neighbor_nodes) const; NodeSharedPtr node_start_; NodeSharedPtr node_goal_; diff --git a/planning/freespace_planning_algorithms/rrtstar.md b/planning/freespace_planning_algorithms/rrtstar.md index d48c065ee9574..8f64fe757bc2f 100644 --- a/planning/freespace_planning_algorithms/rrtstar.md +++ b/planning/freespace_planning_algorithms/rrtstar.md @@ -4,9 +4,11 @@ Let us define $f(x)$ as minimum cost of the path when path is constrained to pass through $x$ (so path will be $x_{\mathrm{start}} \to \mathrm{x} \to \mathrm{x_{\mathrm{goal}}}$). Also, let us define $c_{\mathrm{best}}$ as the current minimum cost of the feasible paths. Let us define a set $ X(f) = \left\{ x \in X | f(x) < c*{\mathrm{best}} \right\} $. If we could sample a new point from $X_f$ instead of $X$ as in vanilla RRT\*, chance that $c*{\mathrm{best}}$ is updated is increased, thus the convergence rate is improved. -In most case, $f(x)$ is unknown, thus it is straightforward to approximiate the function $f$ by a heuristic function $\hat{f}$. A heuristic function is [admissible](https://en.wikipedia.org/wiki/Admissible_heuristic) if $\forall x \in X, \hat{f}(x) < f(x)$, which is sufficient condition of conversion to optimal path. The good heuristic function $\hat{f}$ has two properties: 1) it is an admissible tight lower bound of $f$ and 2) sampling from $X(\hat{f})$ is easy. +In most case, $f(x)$ is unknown, thus it is straightforward to approximate the function $f$ by a heuristic function $\hat{f}$. A heuristic function is [admissible](https://en.wikipedia.org/wiki/Admissible_heuristic) if $\forall x \in X, \hat{f}(x) < f(x)$, which is sufficient condition of conversion to optimal path. The good heuristic function $\hat{f}$ has two properties: 1) it is an admissible tight lower bound of $f$ and 2) sampling from $X(\hat{f})$ is easy. -According to Gammell et al [1], a good heursitic function when path is always straight is $\hat{f}(x) = ||x_{\mathrm{start}} - x|| + ||x - x_{\mathrm{goal}}||$. If we don't assume any obstacle information the heursitic is tightest. Also, $X(\hat{f})$ is hyper-ellipsoid, and hence sampling from it can be done analitically. + + +According to Gammell et al [1], a good heuristic function when path is always straight is $\hat{f}(x) = ||x_{\mathrm{start}} - x|| + ||x - x_{\mathrm{goal}}||$. If we don't assume any obstacle information the heuristic is tightest. Also, $X(\hat{f})$ is hyper-ellipsoid, and hence sampling from it can be done analytically. ### Modification to fit reeds-sheep path case diff --git a/planning/freespace_planning_algorithms/src/rrtstar.cpp b/planning/freespace_planning_algorithms/src/rrtstar.cpp index f03dc81faba05..1607451173f9f 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/freespace_planning_algorithms/src/rrtstar.cpp @@ -17,7 +17,7 @@ namespace { -rrtstar_core::Pose posemsgToPose(const geometry_msgs::msg::Pose & pose_msg) +rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg) { return rrtstar_core::Pose{ pose_msg.position.x, pose_msg.position.y, tf2::getYaw(pose_msg.orientation)}; @@ -68,8 +68,8 @@ bool RRTStar::makePlan( M_PI}; const double radius = planner_common_param_.minimum_turning_radius; const auto cspace = rrtstar_core::CSpace(lo, hi, radius, is_obstacle_free); - const auto x_start = posemsgToPose(start_pose_); - const auto x_goal = posemsgToPose(goal_pose_); + const auto x_start = poseMsgToPose(start_pose_); + const auto x_goal = poseMsgToPose(goal_pose_); if (!is_obstacle_free(x_start)) { return false; @@ -82,7 +82,7 @@ bool RRTStar::makePlan( const bool is_informed = rrtstar_param_.use_informed_sampling; // always better const double collision_check_resolution = rrtstar_param_.margin * 2; auto algo = rrtstar_core::RRTStar( - x_start, x_goal, rrtstar_param_.neighbour_radius, collision_check_resolution, is_informed, + x_start, x_goal, rrtstar_param_.neighbor_radius, collision_check_resolution, is_informed, cspace); while (true) { const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); @@ -153,15 +153,15 @@ void RRTStar::setRRTPath(const std::vector & waypoints) if (0 == i) { const auto & pt_now = waypoints.at(i); const auto & pt_next = waypoints.at(i + 1); - const double inpro = + const double inner_product = cos(pt_now.yaw) * (pt_next.x - pt_now.x) + sin(pt_now.yaw) * (pt_next.y - pt_now.y); - pw.is_back = (inpro < 0.0); + pw.is_back = (inner_product < 0.0); } else { const auto & pt_pre = waypoints.at(i - 1); const auto & pt_now = waypoints.at(i); - const double inpro = + const double inner_product = cos(pt_pre.yaw) * (pt_now.x - pt_pre.x) + sin(pt_pre.yaw) * (pt_now.y - pt_pre.y); - pw.is_back = !(inpro > 0.0); + pw.is_back = !(inner_product > 0.0); } pw.pose = pose; waypoints_.waypoints.push_back(pw); diff --git a/planning/freespace_planning_algorithms/src/rrtstar_core.cpp b/planning/freespace_planning_algorithms/src/rrtstar_core.cpp index 5ccf1d0641930..25aa54a4f19ab 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar_core.cpp +++ b/planning/freespace_planning_algorithms/src/rrtstar_core.cpp @@ -24,6 +24,8 @@ #include #include +// cspell: ignore rsspace +// In this case, RSSpace means "Reeds Shepp state Space" namespace rrtstar_core { CSpace::CSpace( @@ -158,13 +160,13 @@ void RRTStar::extend() return; } - const auto & neighbore_nodes = findNeighboreNodes(x_new); + const auto & neighbor_nodes = findNeighborNodes(x_new); - const auto & node_best_parent = getBestParentNode(x_new, node_nearest, neighbore_nodes); + const auto & node_best_parent = getBestParentNode(x_new, node_nearest, neighbor_nodes); const auto & node_new = addNewNode(x_new, std::const_pointer_cast(node_best_parent)); // Rewire - const auto node_reconnect = getReconnectTargeNode(node_new, neighbore_nodes); + const auto node_reconnect = getReconnectTargeNode(node_new, neighbor_nodes); if (node_reconnect) { reconnect(node_new, std::const_pointer_cast(node_reconnect)); } @@ -197,7 +199,8 @@ void RRTStar::extend() void RRTStar::deleteNodeUsingBranchAndBound() { - // see III.B of Karaman et al. ICRA 2011 + // cspell: ignore Karaman + // ref : III.B of Karaman et al. ICRA 2011 if (!isSolutionFound()) { return; } @@ -341,26 +344,26 @@ NodeConstSharedPtr RRTStar::findNearestNode(const Pose & x_rand) const return node_nearest; } -std::vector RRTStar::findNeighboreNodes(const Pose & x_new) const +std::vector RRTStar::findNeighborNodes(const Pose & x_new) const { // In the original paper of rrtstar, radius is shrinking over time. // However, because we use reeds-shepp distance metric instead of Euclidean metric, - // it is hard to design the shirinking radius update. Usage of informed sampling - // makes the problem far more complex, as the sampling reagion is shirinking over + // it is hard to design the shrinking radius update. Usage of informed sampling + // makes the problem far more complex, as the sampling region is shrinking over // the time. // Due to above difficulty in design of radius update, radius is simply fixed here. // In practice, the fixed radius setting works well as long as mu_ value is - // propery tuned. In car planning scenario, the order or planning world area - // is similar, and turining radius is also similar through different problems. + // properly tuned. In car planning scenario, the order or planning world area + // is similar, and turning radius is also similar through different problems. // So, tuning mu_ parameter is not so difficult. - const double radius_neighbore = mu_; + const double radius_neighbor = mu_; std::vector nodes; for (auto & node : nodes_) { - if (cspace_.distanceLowerBound(node->pose, x_new) > radius_neighbore) continue; - const bool is_neighbour = (cspace_.distance(node->pose, x_new) < radius_neighbore); - if (is_neighbour) { + if (cspace_.distanceLowerBound(node->pose, x_new) > radius_neighbor) continue; + const bool is_neighbor = (cspace_.distance(node->pose, x_new) < radius_neighbor); + if (is_neighbor) { nodes.push_back(node); } } @@ -379,17 +382,17 @@ NodeSharedPtr RRTStar::addNewNode(const Pose & pose, NodeSharedPtr node_parent) } NodeConstSharedPtr RRTStar::getReconnectTargeNode( - const NodeConstSharedPtr node_new, const std::vector & neighbore_nodes) const + const NodeConstSharedPtr node_new, const std::vector & neighbor_nodes) const { NodeConstSharedPtr node_reconnect = nullptr; - for (const auto & node_neighbore : neighbore_nodes) { + for (const auto & node_neighbor : neighbor_nodes) { if (cspace_.isValidPath_child2parent( - node_neighbore->pose, node_new->pose, collision_check_resolution_)) { + node_neighbor->pose, node_new->pose, collision_check_resolution_)) { const double cost_from_start_rewired = - *node_new->cost_from_start + cspace_.distance(node_new->pose, node_neighbore->pose); - if (cost_from_start_rewired < *node_neighbore->cost_from_start) { - node_reconnect = node_neighbore; + *node_new->cost_from_start + cspace_.distance(node_new->pose, node_neighbor->pose); + if (cost_from_start_rewired < *node_neighbor->cost_from_start) { + node_reconnect = node_neighbor; } } } @@ -399,12 +402,12 @@ NodeConstSharedPtr RRTStar::getReconnectTargeNode( NodeConstSharedPtr RRTStar::getBestParentNode( const Pose & pose_new, const NodeConstSharedPtr & node_nearest, - const std::vector & neighbore_nodes) const + const std::vector & neighbor_nodes) const { NodeConstSharedPtr node_best = node_nearest; double cost_min = *(node_nearest->cost_from_start) + cspace_.distance(node_nearest->pose, pose_new); - for (const auto & node : neighbore_nodes) { + for (const auto & node : neighbor_nodes) { const double cost_start_to_new = *(node->cost_from_start) + cspace_.distance(node->pose, pose_new); if (cost_start_to_new < cost_min) { @@ -444,14 +447,14 @@ void RRTStar::reconnect(const NodeSharedPtr & node_new, const NodeSharedPtr & no // node_reconnect_parent -> #nil; // update cost of all descendents of node_reconnect - std::queue bfqueue; - bfqueue.push(node_reconnect); - while (!bfqueue.empty()) { - const auto node = bfqueue.front(); - bfqueue.pop(); + std::queue bf_queue; + bf_queue.push(node_reconnect); + while (!bf_queue.empty()) { + const auto node = bf_queue.front(); + bf_queue.pop(); for (const auto & child : node->childs) { child->cost_from_start = *node->cost_from_start + *child->cost_to_parent; - bfqueue.push(child); + bf_queue.push(child); } } } diff --git a/planning/freespace_planning_algorithms/test/debug_plot.py b/planning/freespace_planning_algorithms/test/debug_plot.py index 7d6277a2756c6..bc96963f09dcb 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot.py +++ b/planning/freespace_planning_algorithms/test/debug_plot.py @@ -91,8 +91,10 @@ class VehicleModel: def from_problem_description(cls, pd: ProblemDescription) -> "VehicleModel": return cls(pd.vehicle_length.data, pd.vehicle_width.data, pd.vehicle_base2back.data) + # cspell: ignore nparr + # nparr means "numpy array" (maybe) def get_vertices(self, pose: Pose) -> np.ndarray: - x, y, yaw = self.posemsg_to_nparr(pose) + x, y, yaw = self.pose_msg_to_nparr(pose) back = -1.0 * self.base2back front = self.length - self.base2back @@ -120,20 +122,20 @@ def euler_from_quaternion(quaternion): z = quaternion.z w = quaternion.w - sinr_cosp = 2 * (w * x + y * z) - cosr_cosp = 1 - 2 * (x * x + y * y) - roll = atan2(sinr_cosp, cosr_cosp) + sin_roll_cos_pitch = 2 * (w * x + y * z) + cos_roll_cos_pitch = 1 - 2 * (x * x + y * y) + roll = atan2(sin_roll_cos_pitch, cos_roll_cos_pitch) - sinp = 2 * (w * y - z * x) - pitch = asin(sinp) + sin_pitch = 2 * (w * y - z * x) + pitch = asin(sin_pitch) - siny_cosp = 2 * (w * z + x * y) - cosy_cosp = 1 - 2 * (y * y + z * z) - yaw = atan2(siny_cosp, cosy_cosp) + sin_yaw_cos_pitch = 2 * (w * z + x * y) + cos_yaw_cos_pitch = 1 - 2 * (y * y + z * z) + yaw = atan2(sin_yaw_cos_pitch, cos_yaw_cos_pitch) return roll, pitch, yaw @staticmethod - def posemsg_to_nparr(pose_msg: Pose) -> Tuple[float, float, float]: + def pose_msg_to_nparr(pose_msg: Pose) -> Tuple[float, float, float]: _, _, yaw = VehicleModel.euler_from_quaternion(pose_msg.orientation) return pose_msg.position.x, pose_msg.position.y, yaw @@ -151,12 +153,12 @@ def plot_problem(pd: ProblemDescription, ax, meta_info): X, Y = np.meshgrid(x_lin, y_lin) ax.contourf(X, Y, arr, cmap="Greys") - vmodel = VehicleModel.from_problem_description(pd) - vmodel.plot_pose(pd.start, ax, "green") - vmodel.plot_pose(pd.goal, ax, "red") + vehicle_model = VehicleModel.from_problem_description(pd) + vehicle_model.plot_pose(pd.start, ax, "green") + vehicle_model.plot_pose(pd.goal, ax, "red") for pose in pd.trajectory.poses: - vmodel.plot_pose(pose, ax, "blue", 0.5) + vehicle_model.plot_pose(pose, ax, "blue", 0.5) text = "elapsed : {0} [msec]".format(int(round(pd.elapsed_time.data))) ax.text(0.3, 0.3, text, fontsize=15, color="red") @@ -168,7 +170,7 @@ def plot_problem(pd: ProblemDescription, ax, meta_info): ax.set_ylim([b_min[1], b_max[1]]) -def create_concate_png(src_list, dest, is_horizontal): +def create_concat_png(src_list, dest, is_horizontal): opt = "+append" if is_horizontal else "-append" cmd = ["convert", opt] for src in src_list: @@ -179,11 +181,14 @@ def create_concate_png(src_list, dest, is_horizontal): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument("--concat", action="store_true", help="concat pngs (requires image magick)") + parser.add_argument( + "--concat", action="store_true", help="concat png images (requires imagemagick)" + ) args = parser.parse_args() concat = args.concat dir_name_table: Dict[Tuple[str, int], str] = {} + # cspell: ignore fpalgos, cand prefix = "fpalgos" for cand_dir in os.listdir("/tmp"): if cand_dir.startswith(prefix): @@ -200,7 +205,7 @@ def create_concate_png(src_list, dest, is_horizontal): for i in range(n_algo): algo_name = algo_names[i] - algo_pngs = [] + algo_png_images = [] for j in range(n_case): fig, ax = plt.subplots() @@ -214,10 +219,10 @@ def create_concate_png(src_list, dest, is_horizontal): fig.tight_layout() file_name = os.path.join("/tmp", "plot-{}.png".format(meta_info)) - algo_pngs.append(file_name) + algo_png_images.append(file_name) plt.savefig(file_name) print("saved to {}".format(file_name)) algowise_summary_file = os.path.join("/tmp", "summary-{}.png".format(algo_name)) if concat: - create_concate_png(algo_pngs, algowise_summary_file, True) + create_concat_png(algo_png_images, algowise_summary_file, True) diff --git a/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py b/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py index 3fa6d152eba11..f1155e7aed218 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py +++ b/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py @@ -25,6 +25,8 @@ import matplotlib.pyplot as plt import numpy as np +# cspell: ignore ndim, ndata, linewidth + @dataclass class Node: diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 8ea9e0b4af8b5..a7fdab0df7b8f 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -35,9 +35,9 @@ namespace fpa = freespace_planning_algorithms; -const double length_lexas = 5.5; -const double width_lexas = 2.75; -const fpa::VehicleShape vehicle_shape = fpa::VehicleShape{length_lexas, width_lexas, 1.5}; +const double length_lexus = 5.5; +const double width_lexus = 2.75; +const fpa::VehicleShape vehicle_shape = fpa::VehicleShape{length_lexus, width_lexus, 1.5}; const double pi = 3.1415926; const std::array start_pose{5.5, 4., pi * 0.5}; const std::array goal_pose1{8.0, 26.3, pi * 1.5}; // easiest @@ -114,22 +114,22 @@ nav_msgs::msg::OccupancyGrid construct_cost_map( } // car1 - if (10.0 < x && x < 10.0 + width_lexas && 22.0 < y && y < 22.0 + length_lexas) { + if (10.0 < x && x < 10.0 + width_lexus && 22.0 < y && y < 22.0 + length_lexus) { costmap_msg.data[i * width + j] = 100.0; } // car2 - if (13.5 < x && x < 13.5 + width_lexas && 22.0 < y && y < 22.0 + length_lexas) { + if (13.5 < x && x < 13.5 + width_lexus && 22.0 < y && y < 22.0 + length_lexus) { costmap_msg.data[i * width + j] = 100.0; } // car3 - if (20.0 < x && x < 20.0 + width_lexas && 22.0 < y && y < 22.0 + length_lexas) { + if (20.0 < x && x < 20.0 + width_lexus && 22.0 < y && y < 22.0 + length_lexus) { costmap_msg.data[i * width + j] = 100.0; } // car4 - if (10.0 < x && x < 10.0 + width_lexas && 10.0 < y && y < 10.0 + length_lexas) { + if (10.0 < x && x < 10.0 + width_lexus && 10.0 < y && y < 10.0 + length_lexus) { costmap_msg.data[i * width + j] = 100.0; } } @@ -238,6 +238,7 @@ enum AlgorithmType { RRTSTAR_UPDATE, RRTSTAR_INFORMED_UPDATE, }; +// cspell: ignore fpalgos std::unordered_map rosbag_dir_prefix_table( {{ASTAR_SINGLE, "fpalgos-astar_single"}, {ASTAR_MULTI, "fpalgos-astar_multi"}, @@ -363,7 +364,7 @@ TEST(AstarSearchTestSuite, MultiCurvature) EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_MULTI)); } -TEST(RRTStarTestSuite, Fastetst) { EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_FASTEST)); } +TEST(RRTStarTestSuite, Fastest) { EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_FASTEST)); } TEST(RRTStarTestSuite, Update) { EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_UPDATE)); } diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index e3ddbfb1b157a..81d205c0c1c3e 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -200,7 +200,7 @@ $$ | Variable | Description | | ----------------- | --------------------------------------- | -| `d` | actual distane to obstacle | +| `d` | actual distance to obstacle | | `d_{rss}` | ideal distance to obstacle based on RSS | | `v_{min, cruise}` | `min_cruise_target_vel` | | `w_{acc}` | `output_ratio_during_accel` | diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index f94516407e3c5..e15f8f9759660 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -82,7 +82,7 @@ class PIDBasedPlanner : public PlannerInterface const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info); - // velocityinsertion based planner + // velocity insertion based planner Trajectory doCruiseWithTrajectory( const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info); diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index 3a0807d4e1a5d..d670f07d26e9d 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -21,7 +21,7 @@ If the footprint collides with some obstacle, the velocity at the trajectory poi The motion of the ego vehicle is simulated at each trajectory point using the `heading`, `velocity`, and `steering` defined at the point. Footprints are then constructed from these simulations and checked for collision. -If a collision is found, the distance from the trajectory point is used to calculate the adjusted velocity that would produce a collision-free footprint. Parameter `simulation.distance_method` allow to switch between an exact distance calculation and a less expensive approximation using a simple euclidian distance. +If a collision is found, the distance from the trajectory point is used to calculate the adjusted velocity that would produce a collision-free footprint. Parameter `simulation.distance_method` allow to switch between an exact distance calculation and a less expensive approximation using a simple euclidean distance. Two models can be selected with parameter `simulation.model` for simulating the motion of the vehicle: a simple particle model and a more complicated bicycle model. @@ -127,7 +127,7 @@ If a collision is found, the velocity at the trajectory point is adjusted such t $velocity = \frac{dist\_to\_collision}{min\_ttc}$ To prevent sudden deceleration of the ego vehicle, the parameter `max_deceleration` limits the deceleration relative to the current ego velocity. -For a trajectory point occuring at a duration `t` in the future (calculated from the original velocity profile), +For a trajectory point occurring at a duration `t` in the future (calculated from the original velocity profile), the adjusted velocity cannot be set lower than $v_{current} - t * max\_deceleration$. Furthermore, a parameter `min_adjusted_velocity` diff --git a/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp index 936476c800117..cfd8ea0f3e244 100644 --- a/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp +++ b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp @@ -14,6 +14,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp index 7b9553a936ee7..c33e283ed7c0c 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp @@ -17,6 +17,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp index 22d70463fc0d9..ca80fc468079d 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp @@ -18,6 +18,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp index 9c545e6fe2821..51b5111b9d6ac 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp @@ -17,6 +17,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp index b1e8f848acd88..32b69b149e9f2 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_VELOCITY_LIMITER__MAP_UTILS_HPP_ #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index b95d3849cda9a..91611938119e5 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -18,6 +18,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include #include @@ -102,13 +103,13 @@ polygon_t createEnvelopePolygon(const std::vector & footprints); /// @details depending on the method used, multiple lines can be created for a same trajectory point /// @param[in] trajectory input trajectory /// @param[in] params projection parameters -/// @return projecton lines for each trajectory point +/// @return projection lines for each trajectory point std::vector createProjectedLines( const Trajectory & trajectory, ProjectionParameters & params); /// @brief limit the velocity of the given trajectory /// @param[in] trajectory input trajectory -/// @param[in] collision_checker object used to retrive collision points +/// @param[in] collision_checker object used to retrieve collision points /// @param[in] projections forward projection lines at each trajectory point /// @param[in] footprints footprint of the forward projection at each trajectory point /// @param[in] projection_params projection parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 1575fbcd13423..a74128f57d6fd 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -18,6 +18,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp index 4d82f31dc019c..0d7edc649857d 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -17,6 +17,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp index 9babbe8defb5a..61f4f575f826b 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp @@ -17,6 +17,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp index cb8c812661bfc..7521e6b1a0bba 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_VELOCITY_LIMITER__PARAMETERS_HPP_ #include +// cspell: ignore multipolygon, multilinestring #include #include @@ -39,6 +40,7 @@ struct ObstacleParameters static constexpr auto RTREE_SEGMENTS_PARAM = "obstacles.rtree_min_segments"; static constexpr auto RTREE_POINTS_PARAM = "obstacles.rtree_min_points"; + // cspell: ignore OCCUPANCYGRID enum { POINTCLOUD, OCCUPANCYGRID, STATIC_ONLY } dynamic_source = OCCUPANCYGRID; int8_t occupancy_grid_threshold{}; Float dynamic_obstacles_buffer{}; diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp index 9714a7003b1db..80d79791d0bbc 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp @@ -17,6 +17,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -39,7 +40,7 @@ pcl::PointCloud::Ptr transformPointCloud( const PointCloud & pointcloud_msg, tier4_autoware_utils::TransformListener & transform_listener, const std::string & target_frame); -/// @brief filter the pointcloud to keep only relevent points +/// @brief filter the pointcloud to keep only relevant points /// @param[in,out] pointcloud to filter /// @param[in] masks obstacle masks used to filter the pointcloud void filterPointCloud(pcl::PointCloud::Ptr pointcloud, const ObstacleMasks & masks); diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp index 258ef8f01b5f5..2ee003206339d 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_VELOCITY_LIMITER__TRAJECTORY_PREPROCESSING_HPP_ #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring namespace obstacle_velocity_limiter { diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index f11b2ff87af23..81de7463b9e42 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -23,6 +23,7 @@ #include #include +// cspell: ignore multipolygon, multilinestring namespace obstacle_velocity_limiter { using autoware_auto_perception_msgs::msg::PredictedObjects; diff --git a/planning/obstacle_velocity_limiter/src/debug.cpp b/planning/obstacle_velocity_limiter/src/debug.cpp index fbbf48e69b023..853dee2d20f49 100644 --- a/planning/obstacle_velocity_limiter/src/debug.cpp +++ b/planning/obstacle_velocity_limiter/src/debug.cpp @@ -16,6 +16,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/src/distance.cpp b/planning/obstacle_velocity_limiter/src/distance.cpp index 2e0ff52c1922c..68d634022c5f7 100644 --- a/planning/obstacle_velocity_limiter/src/distance.cpp +++ b/planning/obstacle_velocity_limiter/src/distance.cpp @@ -38,18 +38,18 @@ std::optional distanceToClosestCollision( for (const auto & obs_point : collision_checker.intersections(footprint)) { if (params.distance_method == ProjectionParameters::EXACT) { if (params.model == ProjectionParameters::PARTICLE) { - const auto euclidian_dist = bg::distance(obs_point, projection.front()); + const auto euclidean_dist = bg::distance(obs_point, projection.front()); const auto collision_heading = std::atan2( obs_point.y() - projection.front().y(), obs_point.x() - projection.front().x()); const auto angle = params.heading - collision_heading; - const auto long_dist = std::abs(std::cos(angle)) * euclidian_dist; + const auto long_dist = std::abs(std::cos(angle)) * euclidean_dist; min_dist = std::min(min_dist, long_dist); } else { // BICYCLE model with curved projection min_dist = std::min(min_dist, arcDistance(projection.front(), params.heading, obs_point)); } } else { // APPROXIMATION - const auto euclidian_dist = bg::distance(obs_point, projection.front()); - min_dist = std::min(min_dist, euclidian_dist); + const auto euclidean_dist = bg::distance(obs_point, projection.front()); + min_dist = std::min(min_dist, euclidean_dist); } } if (min_dist != std::numeric_limits::max()) distance = min_dist; diff --git a/planning/obstacle_velocity_limiter/src/forward_projection.cpp b/planning/obstacle_velocity_limiter/src/forward_projection.cpp index b4000389b19e8..d3193eefe0114 100644 --- a/planning/obstacle_velocity_limiter/src/forward_projection.cpp +++ b/planning/obstacle_velocity_limiter/src/forward_projection.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/src/map_utils.cpp b/planning/obstacle_velocity_limiter/src/map_utils.cpp index 5e39daef89c64..e84734355268e 100644 --- a/planning/obstacle_velocity_limiter/src/map_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/map_utils.cpp @@ -17,6 +17,7 @@ #include "lanelet2_core/primitives/LineString.h" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 15dd0fbadb5bb..6144f338159da 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -21,6 +21,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/trajectory_preprocessing.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include #include diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index ac8444a508be8..417ff6b7783e2 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -79,6 +79,7 @@ void addSensorObstacles( const ObstacleMasks & masks, tier4_autoware_utils::TransformListener & transform_listener, const std::string & target_frame, const ObstacleParameters & obstacle_params) { + // cspell: ignore OCCUPANCYGRID if (obstacle_params.dynamic_source == ObstacleParameters::OCCUPANCYGRID) { auto grid_map = convertToGridMap(occupancy_grid); threshold(grid_map, obstacle_params.occupancy_grid_threshold); diff --git a/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp index 72b9bf6431f03..397e20d05ebe0 100644 --- a/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp @@ -15,6 +15,7 @@ #include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include #include diff --git a/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp index 4f9ca6c5bc3e7..25339a49529d6 100644 --- a/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp @@ -15,6 +15,7 @@ #include "obstacle_velocity_limiter/pointcloud_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index e9cf8d9ad4dd6..64693ef8ef249 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -15,6 +15,7 @@ #include "obstacle_velocity_limiter/distance.hpp" #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp index 3275bf1ab5506..6e35ec95c62e0 100644 --- a/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp +++ b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp @@ -14,6 +14,7 @@ #include "obstacle_velocity_limiter/forward_projection.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/obstacle_velocity_limiter/test/test_obstacles.cpp b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp index 70a0e94491b19..f75e808828ca6 100644 --- a/planning/obstacle_velocity_limiter/test/test_obstacles.cpp +++ b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp @@ -14,6 +14,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp index 486dfe74eed73..a6ff8543f6ad0 100644 --- a/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp +++ b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp @@ -14,6 +14,7 @@ #include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" +// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/Readme.md index f2d29c796c747..6538bf1db222d 100644 --- a/planning/planning_debug_tools/Readme.md +++ b/planning/planning_debug_tools/Readme.md @@ -77,6 +77,9 @@ PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_sta in Function Library ![image](./image/script.png) + + + ```lua function PlotValue(name, path, timestamp, value) diff --git a/planning/planning_debug_tools/scripts/closest_velocity_checker.py b/planning/planning_debug_tools/scripts/closest_velocity_checker.py index 7051cf31bc6a9..292a2ae49ae9c 100755 --- a/planning/planning_debug_tools/scripts/closest_velocity_checker.py +++ b/planning/planning_debug_tools/scripts/closest_velocity_checker.py @@ -69,8 +69,8 @@ def __init__(self): # planning path and trajectories profile = rclpy.qos.QoSProfile(depth=1) - transien_local = rclpy.qos.QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL - transient_local_profile = rclpy.qos.QoSProfile(depth=1, durability=transien_local) + transient_local = rclpy.qos.QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL + transient_local_profile = rclpy.qos.QoSProfile(depth=1, durability=transient_local) lane_drv = "/planning/scenario_planning/lane_driving" scenario = "/planning/scenario_planning" self.sub0 = self.create_subscription( diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 4497ee1d77eee..9b5775642424c 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -38,6 +38,7 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in void calcCurvature(const Trajectory & trajectory, std::vector & curvatures); +// cspell: ignore steerings void calcSteeringAngles( const Trajectory & trajectory, const double wheelbase, std::vector & steerings); diff --git a/sensing/geo_pos_conv/src/geo_pos_conv.cpp b/sensing/geo_pos_conv/src/geo_pos_conv.cpp index 119424bf19db0..a046b73f65a16 100644 --- a/sensing/geo_pos_conv/src/geo_pos_conv.cpp +++ b/sensing/geo_pos_conv/src/geo_pos_conv.cpp @@ -211,7 +211,7 @@ void geo_pos_conv::conv_llh2xyz(void) Pmo = 0.9999; /*WGS84 Parameters*/ - AW = 6378137.0; // Semimajor Axis + AW = 6378137.0; // Semi-major Axis FW = 1.0 / 298.257222101; // 298.257223563 //Geometrical flattening Pe = sqrt(2.0 * FW - pow(FW, 2)); diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 1c3c0dbb6d602..1ad25ae35a124 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -30,8 +30,8 @@ namespace gnss_poser { enum class MGRSPrecision { - _10_KIRO_METER = 1, - _1_KIRO_METER = 2, + _10_KILO_METER = 1, + _1_KILO_METER = 2, _100_METER = 3, _10_METER = 4, _1_METER = 5, @@ -87,7 +87,8 @@ GNSSStat NavSatFix2UTM( try { GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.northup, utm.x, utm.y); + nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, + utm.y); utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); @@ -111,14 +112,14 @@ GNSSStat NavSatFix2LocalCartesianUTM( utm_origin.coordinate_system = CoordinateSystem::UTM; GeographicLib::UTMUPS::Forward( nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, - utm_origin.northup, utm_origin.x, utm_origin.y); + utm_origin.east_north_up, utm_origin.x, utm_origin.y); utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger); // individual coordinates of global coordinate system double global_x = 0.0; double global_y = 0.0; GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, utm_origin.northup, - global_x, global_y); + nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, + utm_origin.east_north_up, global_x, global_y); utm_local.latitude = nav_sat_fix_msg.latitude; utm_local.longitude = nav_sat_fix_msg.longitude; utm_local.altitude = nav_sat_fix_msg.altitude; @@ -142,7 +143,8 @@ GNSSStat UTM2MGRS( try { std::string mgrs_code; GeographicLib::MGRS::Forward( - utm.zone, utm.northup, utm.x, utm.y, utm.latitude, static_cast(precision), mgrs_code); + utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), + mgrs_code); mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * std::pow( diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index 030e569d69a3d..46fdc6eeff9ee 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -30,7 +30,7 @@ struct GNSSStat { GNSSStat() : coordinate_system(CoordinateSystem::MGRS), - northup(true), + east_north_up(true), zone(0), mgrs_zone(""), x(0), @@ -43,7 +43,7 @@ struct GNSSStat } CoordinateSystem coordinate_system; - bool northup; + bool east_north_up; int zone; std::string mgrs_zone; double x; diff --git a/sensing/image_diagnostics/README.md b/sensing/image_diagnostics/README.md index ff033d6874bb1..03858088564b3 100644 --- a/sensing/image_diagnostics/README.md +++ b/sensing/image_diagnostics/README.md @@ -8,7 +8,7 @@ The `image_diagnostics` is a node that check the status of the input raw image. Below figure shows the flowchart of image diagnostics node. Each image is divided into small blocks for block state assessment. -![image diagnostics flowchar ](./image/image_diagnostics_overview.svg) +![image diagnostics flowchart ](./image/image_diagnostics_overview.svg) Each small image block state is assessed as below figure. diff --git a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml index 7f7f0cdefdb8e..5da1ec3720fb0 100644 --- a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml +++ b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml @@ -49,7 +49,7 @@ - + diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 6c51755768320..519bd831fd39c 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -50,7 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ## Assumptions / Known limits -1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is neccessary to check order of channel id in vertical distribution manually and modifiy the code. +1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in vertical distribution manually and modify the code. 2. The current method is still limited for dust type of blockage when dust particles are sparsely distributed. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 2aaf9d5a4f206..dcb03cc792cae 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -9,7 +9,7 @@ Since the LiDAR sensor scans by rotating an internal laser, the resulting point ## Inner-workings / Algorithms - Use the equations below (specific to the Velodyne 32C sensor) to obtain an accurate timestamp for each scan data point. -- Use twist information to determine the distance the ego-vehicle has travelled between the time that the scan started and the corrected timestamp of each point, and then correct the position of the point. +- Use twist information to determine the distance the ego-vehicle has traveled between the time that the scan started and the corrected timestamp of each point, and then correct the position of the point. The offset equation is given by $ TimeOffset = (55.296 \mu s _SequenceIndex) + (2.304 \mu s_ DataPointIndex) $ diff --git a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg b/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg index e1d0673d61eff..c8cb4e9c57a5f 100644 --- a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg +++ b/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg @@ -58,14 +58,14 @@ >
- no retrurn region + no return region
detection
- no retrurn region... + no return region... @@ -82,7 +82,7 @@
small segment
- fillter + filter
@@ -179,11 +179,11 @@
-
diagnotics
+
diagnostics
 - diagnotics + diagnostics
- Frist return (weak) + First return (weak)
- Frist retur... + First retur... diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp index 5336efb7d81e9..8115a46c1ffbc 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp @@ -62,6 +62,7 @@ namespace pcl { +// cspell: ignore ptfilter /** \brief @b PassThroughUInt16 passes points in a cloud based on constraints for one particular * field of the point type. \details Iterates through the entire input once, automatically filtering * non-finite points and the points outside the interval specified by setFilterLimits(), which diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index 05a404c635ee6..139cf56dccce7 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -186,7 +186,7 @@ void DualReturnOutlierFilterComponent::filter( } else if (keep_next) { temp_segment.points.push_back(*iter); keep_next = false; - // Analyse segment points here + // Analyze segment points here } else { // Log the deleted azimuth and its distance for analysis switch (roi_mode_map_[roi_mode_]) { @@ -220,7 +220,7 @@ void DualReturnOutlierFilterComponent::filter( } } } - // Analyse last segment points here + // Analyze last segment points here std::vector noise_frequency(horizontal_bins, 0); uint current_deleted_index = 0; uint current_temp_segment_index = 0; @@ -301,7 +301,7 @@ void DualReturnOutlierFilterComponent::filter( } else if (keep_next) { temp_segment.points.push_back(*iter); keep_next = false; - // Analyse segment points here + // Analyze segment points here } else { // Log the deleted azimuth and its distance for analysis // deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp index cd589e252544b..3a40408bcd8de 100644 --- a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp +++ b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp @@ -137,6 +137,7 @@ void pcl::PassThroughUInt16::applyFilter(PCLPointCloud2 & o return; } + // cspell: ignore badpt std::uint16_t badpt = user_filter_value_; // Check whether we need to store filtered valued in place if (keep_organized_) { @@ -157,7 +158,7 @@ void pcl::PassThroughUInt16::applyFilter(PCLPointCloud2 & o if (filter_limit_negative_) { // Use a threshold for cutting out points which inside the interval if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) { - // Unoptimized memcpys: assume fields x, y, z are in random order + // Unoptimized memcpy functions: assume fields x, y, z are in random order memcpy(&output.data[xyz_offset[0]], &badpt, sizeof(std::uint16_t)); memcpy(&output.data[xyz_offset[1]], &badpt, sizeof(std::uint16_t)); memcpy(&output.data[xyz_offset[2]], &badpt, sizeof(std::uint16_t)); @@ -170,7 +171,7 @@ void pcl::PassThroughUInt16::applyFilter(PCLPointCloud2 & o } else { // Use a threshold for cutting out points which are too close/far away if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) { - // Unoptimized memcpys: assume fields x, y, z are in random order + // Unoptimized memcpy functions: assume fields x, y, z are in random order memcpy(&output.data[xyz_offset[0]], &badpt, sizeof(std::uint16_t)); memcpy(&output.data[xyz_offset[1]], &badpt, sizeof(std::uint16_t)); memcpy(&output.data[xyz_offset[2]], &badpt, sizeof(std::uint16_t)); @@ -220,7 +221,7 @@ void pcl::PassThroughUInt16::applyFilter(PCLPointCloud2 & o } } - // Unoptimized memcpys: assume fields x, y, z are in random order + // Unoptimized memcpy functions: assume fields x, y, z are in random order memcpy(&pt[0], &input_->data[xyz_offset[0]], sizeof(std::uint16_t)); memcpy(&pt[1], &input_->data[xyz_offset[1]], sizeof(std::uint16_t)); memcpy(&pt[2], &input_->data[xyz_offset[2]], sizeof(std::uint16_t)); @@ -245,7 +246,7 @@ void pcl::PassThroughUInt16::applyFilter(PCLPointCloud2 & o } else { // No distance filtering, process all data. // No need to check for is_organized here as we did it above for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) { - // Unoptimized memcpys: assume fields x, y, z are in random order + // Unoptimized memcpy functions: assume fields x, y, z are in random order memcpy(&pt[0], &input_->data[xyz_offset[0]], sizeof(std::uint16_t)); memcpy(&pt[1], &input_->data[xyz_offset[1]], sizeof(std::uint16_t)); memcpy(&pt[2], &input_->data[xyz_offset[2]], sizeof(std::uint16_t)); diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp index 670e58fb936cc..44b2f0dfe6ef0 100644 --- a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp @@ -94,6 +94,7 @@ class VoxelGridNearestCentroid : public VoxelGrid using VoxelGrid::max_b_; using VoxelGrid::inverse_leaf_size_; using VoxelGrid::div_b_; + // cspell: ignore divb using VoxelGrid::divb_mul_; typedef typename pcl::traits::fieldList::type FieldList; @@ -216,7 +217,7 @@ class VoxelGridNearestCentroid : public VoxelGrid : searchable_(true), // min_points_per_voxel_ (6), min_points_per_voxel_(1), - // min_covar_eigvalue_mult_ (0.01), + // min_covar_eigenvalue_mult_ (0.01), leaves_(), voxel_centroids_(), voxel_centroids_leaf_indices_(), @@ -257,12 +258,12 @@ class VoxelGridNearestCentroid : public VoxelGrid inline int getMinPointPerVoxel() { return min_points_per_voxel_; } /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance - * matrices. \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues + * matrices. \param[in] min_covar_eigenvalue_mult the minimum allowable ratio between eigenvalues */ // inline void - // setCovEigValueInflationRatio (double min_covar_eigvalue_mult) + // setCovEigValueInflationRatio (double min_covar_eigenvalue_mult) // { - // min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; + // min_covar_eigenvalue_mult_ = min_covar_eigenvalue_mult; // } /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance @@ -271,7 +272,7 @@ class VoxelGridNearestCentroid : public VoxelGrid // inline double // getCovEigValueInflationRatio () // { - // return min_covar_eigvalue_mult_; + // return min_covar_eigenvalue_mult_; // } /** \brief Filter cloud and initializes voxel structure. @@ -516,7 +517,7 @@ class VoxelGridNearestCentroid : public VoxelGrid /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance * matrices. */ - // double min_covar_eigvalue_mult_; + // double min_covar_eigenvalue_mult_; /** \brief Voxel structure containing all leaf nodes (includes voxels with less than * a sufficient number of points). */ diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp index bd285e17edac7..6298c4a2e4153 100644 --- a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp @@ -126,6 +126,7 @@ void pcl::VoxelGridNearestCentroid::applyFilter(PointCloud & output) // Clear the leaves leaves_.clear(); + // cspell: ignore divb // Set up the division multiplier divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0); @@ -299,7 +300,7 @@ void pcl::VoxelGridNearestCentroid::applyFilter(PointCloud & output) // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the // max eigen value. - // double min_covar_eigvalue; + // double min_covar_eigenvalue; for (typename std::map::iterator it = leaves_.begin(); it != leaves_.end(); ++it) { // Normalize the centroid diff --git a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp index 0e61f2d1122dc..e20c19da93af0 100644 --- a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp +++ b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp @@ -53,7 +53,7 @@ double BoxSDF::operator()(double x, double y) const const auto && vec_global = tf2::Vector3(x, y, 0.0); const auto vec_local = tf_local_to_global_(vec_global); - // As for signed distance field for a box, please refere: + // As for signed distance field for a box, please refer: // https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm const auto sd_val_x = std::abs(vec_local.getX()) - 0.5 * length_; const auto sd_val_y = std::abs(vec_local.getY()) - 0.5 * width_; diff --git a/tools/simulator_test/simulator_compatibility_test/README.md b/tools/simulator_test/simulator_compatibility_test/README.md index 2b6f2a3b8928c..5dc9d233ce19d 100644 --- a/tools/simulator_test/simulator_compatibility_test/README.md +++ b/tools/simulator_test/simulator_compatibility_test/README.md @@ -13,8 +13,8 @@ File structure - test_morai_sim 1. test_base provides shared methods for testing. Other test codes are created based on functions defined here. -2. test_sim_common_manual_testing provides the most basic functions. Any simualtor can be tested using codes here. However, to make these codes usable with any simulators, the codes do not include any features for test automation. -3. test_morai_sim is an automated version of test_sim_common_manual_testing for MORAI SIM: Drive. Thus it includes 'MORAI SIM: Drive'-specific codes. Users of the other simulators may create similar version for thier simulator of interest. +2. test_sim_common_manual_testing provides the most basic functions. Any simulator can be tested using codes here. However, to make these codes usable with any simulators, the codes do not include any features for test automation. +3. test_morai_sim is an automated version of test_sim_common_manual_testing for MORAI SIM: Drive. Thus it includes 'MORAI SIM: Drive'-specific codes. Users of the other simulators may create similar version for their simulator of interest. ## Test Procedures for test_sim_common_manual_testing diff --git a/tools/simulator_test/simulator_compatibility_test/setup.py b/tools/simulator_test/simulator_compatibility_test/setup.py index 93ce1ab547e7b..f5b1e18cfdaf7 100644 --- a/tools/simulator_test/simulator_compatibility_test/setup.py +++ b/tools/simulator_test/simulator_compatibility_test/setup.py @@ -4,6 +4,8 @@ from setuptools import SetuptoolsDeprecationWarning from setuptools import setup +# cspell: ignore moraisim + simplefilter("ignore", category=SetuptoolsDeprecationWarning) simplefilter("ignore", category=PkgResourcesDeprecationWarning) diff --git a/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/morai_ctrl_cmd.py b/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/morai_ctrl_cmd.py index c8822d6f3194a..a1af495cd02bd 100644 --- a/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/morai_ctrl_cmd.py +++ b/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/morai_ctrl_cmd.py @@ -18,6 +18,7 @@ def __init__(self): self.topic = "/ctrl_cmd" self.publisher_ = self.create_publisher(CtrlCmd, self.topic, 10) + # cspell: ignore longl def publish_msg(self, cmd): msg = CtrlCmd() msg.longl_cmd_type = cmd["longCmdType"] diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md index c7b6432686218..b594f389db34b 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md @@ -243,6 +243,7 @@ Update the offsets by RLS in four grids around newly obtained data. By consideri **Advantage** : No data is wasted because updates are performed on the 4 grids around the data with appropriate weighting. **Disadvantage** : Accuracy may be degraded due to extreme bias of the data. For example, if data $z(k)$ is biased near $Z_{RR}$ in Fig. 2, updating is performed at the four surrounding points ( $Z_{RR}$, $Z_{RL}$, $Z_{LR}$, and $Z_{LL}$), but accuracy at $Z_{LL}$ is not expected. +

@@ -253,4 +254,6 @@ See eq.(7)-(10) in [1] for the updated formula. In addition, eq.(17),(18) from [ ### References + + [1] [Gabrielle Lochrie, Michael Doljevic, Mario Nona, Yongsoon Yoon, Anti-Windup Recursive Least Squares Method for Adaptive Lookup Tables with Application to Automotive Powertrain Control Systems, IFAC-PapersOnLine, Volume 54, Issue 20, 2021, Pages 840-845](https://www.sciencedirect.com/science/article/pii/S240589632102320X) diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index fb0bd00794c52..e45439ecb4d41 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -144,7 +144,7 @@ double RawVehicleCommandConverterNode::calculateSteer( double dt = (current_time - prev_time_steer_calculation_).seconds(); if (std::abs(dt) > 1.0) { RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "ignore old topic"); - dt = 0.1; // set ordinaray delta time instead + dt = 0.1; // set ordinary delta time instead } prev_time_steer_calculation_ = current_time; // feed-forward From a9faeda22e6d994fa528341bb623bce79cf93ef9 Mon Sep 17 00:00:00 2001 From: Takahiro Ishikawa Date: Fri, 3 Mar 2023 12:45:47 +0900 Subject: [PATCH 17/24] fix(crop_box_filter): fix bug (#2992) * Fix bug Signed-off-by: Takahiro Ishikawa * Fix bug Signed-off-by: Takahiro Ishikawa --------- Signed-off-by: Takahiro Ishikawa --- .../src/crop_box_filter/crop_box_filter_nodelet.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index da161c39a16ec..949bd8952d837 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -122,7 +122,7 @@ void CropBoxFilterComponent::faster_filter( output.data.resize(input->data.size()); size_t output_size = 0; - for (size_t global_offset = 0; global_offset + input->point_step < input->data.size(); + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); global_offset += input->point_step) { Eigen::Vector4f point( *reinterpret_cast(&input->data[global_offset + x_offset]), @@ -143,7 +143,12 @@ void CropBoxFilterComponent::faster_filter( point[1] > param_.min_y && point[1] < param_.max_y && point[0] > param_.min_x && point[0] < param_.max_x; if ((!param_.negative && point_is_inside) || (param_.negative && !point_is_inside)) { - memcpy(&output.data[output_size], &point, input->point_step); + memcpy(&output.data[output_size], &input->data[global_offset], input->point_step); + + *reinterpret_cast(&output.data[output_size + x_offset]) = point[0]; + *reinterpret_cast(&output.data[output_size + y_offset]) = point[1]; + *reinterpret_cast(&output.data[output_size + z_offset]) = point[2]; + output_size += input->point_step; } } From 9929414a6caf96ccfbc1f4c896a2635b89dbbd75 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 3 Mar 2023 14:36:02 +0900 Subject: [PATCH 18/24] fix(avoidance): avoidable right side parked-vehicle (#2932) * feat(route_handler): add getMostRightLanelet() Signed-off-by: satoshi-ota * feat(avoidance): add new param to select right/left hand traffic Signed-off-by: satoshi-ota * fix(avoidance): support right hand traffic Signed-off-by: satoshi-ota * fix(avoidance): avoidable right side parked vehicle Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.cpp | 88 ++++++++++++++----- .../include/route_handler/route_handler.hpp | 9 ++ planning/route_handler/src/route_handler.cpp | 11 +++ 3 files changed, 86 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 848aaf8c8d38c..b188c618d6bb6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -505,37 +505,81 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // +: object position // o: nearest point on centerline - const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); - const auto most_left_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); + bool is_left_side_parked_vehicle = false; + { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); + const auto most_left_lanelet_candidates = + rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); + + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + const lanelet::Attribute 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; + } + } + + const auto center_to_left_boundary = distance2d( + to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + return std::make_pair( + center_to_left_boundary - 0.5 * object.shape.dimensions.y, sub_type); + }(); - 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; + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters_->object_check_min_road_shoulder_width; } - } - const auto center_to_left_boundary = - distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); - double object_shiftable_distance = center_to_left_boundary - 0.5 * object.shape.dimensions.y; + const auto arc_coordinates = toArcCoordinates( + to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + object_data.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; - const lanelet::Attribute sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters_->object_check_min_road_shoulder_width; + is_left_side_parked_vehicle = + object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio; } - const auto arc_coordinates = toArcCoordinates( - to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); - object_data.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; + bool is_right_side_parked_vehicle = false; + { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_right_road_lanelet = rh->getMostRightLanelet(object_closest_lanelet); + const auto most_right_lanelet_candidates = + rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound()); + + lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; + const lanelet::Attribute 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; + } + } + + const auto center_to_right_boundary = distance2d( + to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); + + return std::make_pair( + center_to_right_boundary - 0.5 * object.shape.dimensions.y, sub_type); + }(); + + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters_->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + object_data.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; - const auto is_parking_object = - object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio; + is_right_side_parked_vehicle = + object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio; + } - if (!is_parking_object) { + if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); object_data.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; data.other_objects.push_back(object_data); diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 591b436151cf4..52f8082f125e4 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -180,6 +180,15 @@ class RouteHandler const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, bool is_opposite = true, const bool & invert_opposite = false) const noexcept; + /** + * @brief Check if same-direction lane is available at the right side of the lanelet + * Searches for any lanes regardless of whether it is lane-changeable or not. + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet having same direction if true + */ + lanelet::ConstLanelet getMostRightLanelet(const lanelet::ConstLanelet & lanelet) const; + /** * @brief Check if same-direction lane is available at the left side of the lanelet * Searches for any lanes regardless of whether it is lane-changeable or not. diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index b88cf06d2d3cc..d2ea4c65adaa9 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -984,6 +984,17 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane return opposite_lanelets; } +lanelet::ConstLanelet RouteHandler::getMostRightLanelet(const lanelet::ConstLanelet & lanelet) const +{ + // recursively compute the width of the lanes + const auto & same = getRightLanelet(lanelet); + + if (same) { + return getMostRightLanelet(same.get()); + } + return lanelet; +} + lanelet::ConstLanelet RouteHandler::getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const { // recursively compute the width of the lanes From 844d21033277ef2f8aaf8a794834fda2539bd3ca Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 3 Mar 2023 15:47:38 +0900 Subject: [PATCH 19/24] feat(tier4_planning_rviz_plugin): add maintainer (#2996) Signed-off-by: yutaka --- common/tier4_planning_rviz_plugin/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index f5ae8faa733c0..b4f49033c111b 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -5,6 +5,7 @@ 0.1.0 The tier4_planning_rviz_plugin package Yukihiro Saito + Yutaka Shimizu Takayuki Murooka Apache License 2.0 From 36fcf3f37abd61de234eaa2e2ae618b7077c7956 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 3 Mar 2023 16:05:24 +0900 Subject: [PATCH 20/24] fix(tier4_rviz_planning_plugin): clear objects before return (#2995) * fix(tier4_rviz_planning_plugin): clear objects before return * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../include/path/display.hpp | 5 +++-- .../include/path/display_base.hpp | 12 +++++------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp index 7d9bde57a49c0..63788b8f7f9b3 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display.hpp @@ -127,6 +127,9 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay protected: void visualizeDrivableArea(const typename T::ConstSharedPtr msg_ptr) override { + left_bound_object_->clear(); + right_bound_object_->clear(); + if (!validateBoundFloats(msg_ptr)) { this->setStatus( rviz_common::properties::StatusProperty::Error, "Topic", @@ -134,8 +137,6 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay return; } - left_bound_object_->clear(); - right_bound_object_->clear(); if (property_drivable_area_view_->getBool()) { Ogre::ColourValue color = rviz_common::properties::qtToOgre(property_drivable_area_color_->getColor()); diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 86c6201b81d51..7e48c1721a9e7 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -219,6 +219,11 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay void processMessage(const typename T::ConstSharedPtr msg_ptr) override { + path_manual_object_->clear(); + velocity_manual_object_->clear(); + footprint_manual_object_->clear(); + point_manual_object_->clear(); + if (!validateFloats(msg_ptr)) { this->setStatus( rviz_common::properties::StatusProperty::Error, "Topic", @@ -255,9 +260,6 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay return; } - path_manual_object_->clear(); - velocity_manual_object_->clear(); - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); @@ -392,9 +394,6 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay return; } - footprint_manual_object_->clear(); - point_manual_object_->clear(); - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); @@ -402,7 +401,6 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); footprint_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); point_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); From 7f5195799ffc842abe0943dcdb24f93786ab7b25 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 3 Mar 2023 18:09:16 +0900 Subject: [PATCH 21/24] refactor(behavior_path_planner): remove unnecessary macros (#2998) * fix(behavior_path_planner): remove uncessesary macros Signed-off-by: satoshi-ota * feat(interface): define struct for module config Signed-off-by: satoshi-ota * fix(interface): hide previous_module_output_ Signed-off-by: satoshi-ota * fix(side_shift): split parameters definition Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner_node.hpp | 23 ++++----- .../behavior_path_planner/parameters.hpp | 10 ++++ .../scene_module/scene_module_interface.hpp | 6 ++- .../scene_module_manager_interface.hpp | 11 ++--- .../side_shift/side_shift_module.hpp | 20 +------- .../util/side_shift/side_shift_parameters.hpp | 47 +++++++++++++++++++ .../src/behavior_path_planner_node.cpp | 45 +++++------------- 7 files changed, 88 insertions(+), 74 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/side_shift_parameters.hpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 959b7579e65cb..55b17e08aed2a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -34,6 +34,10 @@ #include "behavior_path_planner/steering_factor_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp" +#include "behavior_path_planner/util/pull_out/pull_out_parameters.hpp" +#include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp" +#include "behavior_path_planner/util/side_shift/side_shift_parameters.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include @@ -107,9 +111,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; -#ifndef USE_OLD_ARCHITECTURE rclcpp::Subscription::SharedPtr operation_mode_subscriber_; -#endif rclcpp::Publisher::SharedPtr path_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; @@ -118,9 +120,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; -#ifndef USE_OLD_ARCHITECTURE std::map::SharedPtr> path_reference_publishers_; -#endif std::shared_ptr planner_data_; @@ -149,23 +149,22 @@ class BehaviorPathPlannerNode : public rclcpp::Node // update planner data std::shared_ptr createLatestPlannerData(); -#ifdef USE_OLD_ARCHITECTURE // parameters - std::shared_ptr avoidance_param_ptr; - std::shared_ptr lane_change_param_ptr; -#endif + std::shared_ptr avoidance_param_ptr_; + std::shared_ptr lane_change_param_ptr_; BehaviorPathPlannerParameters getCommonParam(); #ifdef USE_OLD_ARCHITECTURE BehaviorTreeManagerParam getBehaviorTreeManagerParam(); - SideShiftParameters getSideShiftParam(); - AvoidanceParameters getAvoidanceParam(); LaneFollowingParameters getLaneFollowingParam(); +#endif + + AvoidanceParameters getAvoidanceParam(); LaneChangeParameters getLaneChangeParam(); + SideShiftParameters getSideShiftParam(); PullOverParameters getPullOverParam(); PullOutParameters getPullOutParam(); -#endif // callback void onOdometry(const Odometry::ConstSharedPtr msg); @@ -174,9 +173,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); -#ifndef USE_OLD_ARCHITECTURE void onOperationMode(const OperationModeState::ConstSharedPtr msg); -#endif SetParametersResult onSetParam(const std::vector & parameters); /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index ea38ed2cb61cf..d13f8df4852be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -17,10 +17,20 @@ #include +struct ModuleConfigParameters +{ + bool enable_module{false}; + bool enable_simultaneous_execution{false}; + uint8_t priority{0}; + uint8_t max_module_size{0}; +}; + struct BehaviorPathPlannerParameters { bool verbose; + ModuleConfigParameters config_avoidance; + double backward_path_length; double forward_path_length; double backward_length_buffer_for_end_of_lane; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 28bac155adb98..4d452e0c2e55b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -259,6 +259,8 @@ class SceneModuleInterface rclcpp::Publisher::SharedPtr pub_debug_marker_; #endif + BehaviorModuleOutput previous_module_output_; + protected: void updateRTCStatus(const double start_distance, const double finish_distance) { @@ -277,6 +279,8 @@ class SceneModuleInterface rtc_interface_ptr_->clearCooperateStatus(); } + BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; } @@ -303,8 +307,6 @@ class SceneModuleInterface ModuleStatus current_state_; - BehaviorModuleOutput previous_module_output_; - mutable MarkerArray debug_marker_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 56af2cb2acd7e..d0344c4319c2d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -40,15 +40,14 @@ class SceneModuleManagerInterface { public: SceneModuleManagerInterface( - rclcpp::Node * node, const std::string & name, const size_t max_module_num, - const size_t priority, const bool enable_simultaneous_execution) + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) : node_(node), clock_(*node->get_clock()), logger_(node->get_logger().get_child(name)), name_(name), - max_module_num_(max_module_num), - priority_(priority), - enable_simultaneous_execution_(enable_simultaneous_execution) + max_module_num_(config.max_module_size), + priority_(config.priority), + enable_simultaneous_execution_(config.enable_simultaneous_execution) { pub_debug_marker_ = node->create_publisher("~/debug/" + name, 20); } @@ -157,8 +156,6 @@ class SceneModuleManagerInterface virtual void updateModuleParams(const std::vector & parameters) = 0; protected: - virtual void getModuleParams(rclcpp::Node * node) = 0; - virtual std::shared_ptr createNewSceneModuleInstance() = 0; rclcpp::Node * node_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 9457f01760fff..54c75d27d73ae 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/util/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/util/side_shift/side_shift_parameters.hpp" #include @@ -35,25 +36,6 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using tier4_planning_msgs::msg::LateralOffset; -enum class SideShiftStatus { STOP = 0, BEFORE_SHIFT, SHIFTING, AFTER_SHIFT }; - -struct SideShiftParameters -{ - double time_to_start_shifting; - double min_distance_to_start_shifting; - double shifting_lateral_jerk; - double min_shifting_distance; - double min_shifting_speed; - double drivable_area_resolution; - double drivable_area_width; - double drivable_area_height; - double shift_request_time_limit; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; -}; - class SideShiftModule : public SceneModuleInterface { public: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/side_shift_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/side_shift_parameters.hpp new file mode 100644 index 0000000000000..5a9edd1279f85 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/side_shift_parameters.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ + +#include + +#include +#include + +namespace behavior_path_planner +{ +using tier4_planning_msgs::msg::LateralOffset; + +enum class SideShiftStatus { STOP = 0, BEFORE_SHIFT, SHIFTING, AFTER_SHIFT }; + +struct SideShiftParameters +{ + double time_to_start_shifting; + double min_distance_to_start_shifting; + double shifting_lateral_jerk; + double min_shifting_distance; + double min_shifting_speed; + double drivable_area_resolution; + double drivable_area_width; + double drivable_area_height; + double shift_request_time_limit; + // drivable area expansion + double drivable_area_right_bound_offset; + double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip; +}; + +} // namespace behavior_path_planner +#endif // BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 38a41743c1113..84d0b4a844c32 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -91,12 +91,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod occupancy_grid_subscriber_ = create_subscription( "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); -#ifndef USE_OLD_ARCHITECTURE operation_mode_subscriber_ = create_subscription( "/system/operation_mode/state", 1, std::bind(&BehaviorPathPlannerNode::onOperationMode, this, _1), createSubscriptionOptions(this)); -#endif scenario_subscriber_ = create_subscription( "~/input/scenario", 1, [this](const Scenario::ConstSharedPtr msg) { @@ -113,10 +111,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1), createSubscriptionOptions(this)); -#ifdef USE_OLD_ARCHITECTURE - avoidance_param_ptr = std::make_shared(getAvoidanceParam()); - lane_change_param_ptr = std::make_shared(getLaneChangeParam()); -#endif + // set parameters + { + avoidance_param_ptr_ = std::make_shared(getAvoidanceParam()); + lane_change_param_ptr_ = std::make_shared(getLaneChangeParam()); + } m_set_param_res = this->add_on_set_parameters_callback( std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); @@ -136,7 +135,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_->registerSceneModule(side_shift_module); auto avoidance_module = - std::make_shared("Avoidance", *this, avoidance_param_ptr); + std::make_shared("Avoidance", *this, avoidance_param_ptr_); path_candidate_publishers_.emplace( "Avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); bt_manager_->registerSceneModule(avoidance_module); @@ -147,7 +146,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto ext_request_lane_change_right_module = std::make_shared( - "ExternalRequestLaneChangeRight", *this, lane_change_param_ptr); + "ExternalRequestLaneChangeRight", *this, lane_change_param_ptr_); path_candidate_publishers_.emplace( "ExternalRequestLaneChangeRight", create_publisher(path_candidate_name_space + "ext_request_lane_change_right", 1)); @@ -155,14 +154,14 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto ext_request_lane_change_left_module = std::make_shared( - "ExternalRequestLaneChangeLeft", *this, lane_change_param_ptr); + "ExternalRequestLaneChangeLeft", *this, lane_change_param_ptr_); path_candidate_publishers_.emplace( "ExternalRequestLaneChangeLeft", create_publisher(path_candidate_name_space + "ext_request_lane_change_left", 1)); bt_manager_->registerSceneModule(ext_request_lane_change_left_module); auto lane_change_module = - std::make_shared("LaneChange", *this, lane_change_param_ptr); + std::make_shared("LaneChange", *this, lane_change_param_ptr_); path_candidate_publishers_.emplace( "LaneChange", create_publisher(path_candidate_name_space + "lane_change", 1)); bt_manager_->registerSceneModule(lane_change_module); @@ -224,9 +223,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; -#ifndef USE_OLD_ARCHITECTURE p.verbose = declare_parameter("verbose"); -#endif // vehicle info const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); @@ -305,7 +302,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() return p; } -#ifdef USE_OLD_ARCHITECTURE SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -327,9 +323,7 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() return p; } -#endif -#ifdef USE_OLD_ARCHITECTURE AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() { AvoidanceParameters p{}; @@ -489,7 +483,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() return p; } -#endif #ifdef USE_OLD_ARCHITECTURE LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() @@ -507,7 +500,6 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() } #endif -#ifdef USE_OLD_ARCHITECTURE LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -584,9 +576,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() return p; } -#endif -#ifdef USE_OLD_ARCHITECTURE PullOverParameters BehaviorPathPlannerNode::getPullOverParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -682,9 +672,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() return p; } -#endif -#ifdef USE_OLD_ARCHITECTURE PullOutParameters BehaviorPathPlannerNode::getPullOutParam() { const auto dp = [this](const std::string & str, auto def_val) { @@ -740,7 +728,6 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() return p; } -#endif #ifdef USE_OLD_ARCHITECTURE BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() @@ -785,11 +772,9 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("self_acceleration"); } -#ifndef USE_OLD_ARCHITECTURE if (!planner_data_->operation_mode) { return missing("operation_mode"); } -#endif return true; } @@ -1187,37 +1172,31 @@ void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) route_ptr_ = msg; has_received_route_ = true; } -#ifndef USE_OLD_ARCHITECTURE void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSharedPtr msg) { const std::lock_guard lock(mutex_pd_); planner_data_->operation_mode = msg; } -#endif SetParametersResult BehaviorPathPlannerNode::onSetParam( const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; -#ifdef USE_OLD_ARCHITECTURE - if (!lane_change_param_ptr && !avoidance_param_ptr) { + if (!lane_change_param_ptr_ && !avoidance_param_ptr_) { result.successful = false; result.reason = "param not initialized"; return result; } -#endif result.successful = true; result.reason = "success"; try { -#ifdef USE_OLD_ARCHITECTURE update_param( - parameters, "avoidance.publish_debug_marker", avoidance_param_ptr->publish_debug_marker); + parameters, "avoidance.publish_debug_marker", avoidance_param_ptr_->publish_debug_marker); update_param( - parameters, "lane_change.publish_debug_marker", lane_change_param_ptr->publish_debug_marker); -#endif + parameters, "lane_change.publish_debug_marker", lane_change_param_ptr_->publish_debug_marker); // Drivable area expansion parameters using drivable_area_expansion::DrivableAreaExpansionParameters; update_param( From 9dd2100a7bdd265111cbce7321f98f4c45ed7651 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 3 Mar 2023 21:59:24 +0900 Subject: [PATCH 22/24] refactor(motion_utils): delete calcSignedArcLength with threshold (#2742) * delete calcSignedArcLength_with_threshold Signed-off-by: kyoichi-sugahara * delete calcSignedArcLength_with_threshold Signed-off-by: kyoichi-sugahara * replace called calcSignedArcLength_with_threshold Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add calcSignedArcLength without threshould Signed-off-by: kyoichi-sugahara * add calcSignedArcLength without threshould Signed-off-by: kyoichi-sugahara * modify code in obstacle_cruise_planner Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * modify code in behavior_path_planner Signed-off-by: kyoichi-sugahara * modify code in behavior_velocity_planner Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * fixed code Signed-off-by: kyoichi-sugahara * fixed code Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * add check in case of no segment index found Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../motion_utils/trajectory/trajectory.hpp | 173 +++++------------- .../test/src/trajectory/test_trajectory.cpp | 42 ----- .../pull_over/pull_over_module.cpp | 33 +++- .../src/scene_module/blind_spot/scene.cpp | 13 +- .../occlusion_spot/scene_occlusion_spot.cpp | 11 +- .../optimization_based_planner.cpp | 12 +- .../src/planner_interface.cpp | 35 ++-- 7 files changed, 109 insertions(+), 210 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index cad241673c01b..821507ca9a6a8 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -600,43 +600,6 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } -/** - * @brief calculate length of 2D distance between two points, specified by start pose and end point - * index of points container. - * @param points points of trajectory, path, ... - * @param src_pose start pose - * @param dst_idx index of end point - * @param max_dist max distance, used to search for nearest segment index to start pose - * @param max_yaw max yaw, used to search for nearest segment index to start pose - * @return length of distance between two points. - * Length is positive if destination point associated to dst_idx is greater that point associated to - * src_pose (i.e. after it in trajectory, path, ...) and negative otherwise. - */ -template -boost::optional calcSignedArcLength( - const T & points, const geometry_msgs::msg::Pose & src_pose, const size_t dst_idx, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); - if (!src_seg_idx) { - return boost::none; - } - - const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position); - - return signed_length_on_traj - signed_length_src_offset; -} - /** * @brief calculate length of 2D distance between two points, specified by start index of points * container and end point. @@ -696,47 +659,6 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } -/** - * @brief calculate length of 2D distance between two points, specified by start pose and end point. - * @param points points of trajectory, path, ... - * @param src_pose start pose - * @param dst_point end point - * @param max_dist max distance, used to search for nearest segment index to start pose - * @param max_yaw max yaw, used to search for nearest segment index to start pose - * @return length of distance between two points. - * Length is positive if destination point is greater that source point associated to src_pose (i.e. - * after it in trajectory, path, ...) and negative otherwise. - */ -template -boost::optional calcSignedArcLength( - const T & points, const geometry_msgs::msg::Pose & src_pose, - const geometry_msgs::msg::Point & dst_point, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); - if (!src_seg_idx) { - return boost::none; - } - - const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); - - const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_seg_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); - - return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; -} - /** * @brief calculate length of 2D distance for whole points container, from its start to its end. * @param points points of trajectory, path, ... @@ -838,55 +760,6 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); } -/** - * @brief calculate length of 2D distance between given pose and first point in container with zero - * longitudinal velocity - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @param pose given pose to start the distance calculation from - * @param max_dist max distance, used to search for nearest segment index in points container to the - * given pose - * @param max_yaw max yaw, used to search for nearest segment index in points container to the given - * pose - * @return Length of 2D distance between given pose and first point in container with zero - * longitudinal velocity - */ -template -boost::optional calcDistanceToForwardStopPoint( - const T & points_with_twist, const geometry_msgs::msg::Pose & pose, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - const auto nearest_segment_idx = - motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); - - if (!nearest_segment_idx) { - return boost::none; - } - - const auto stop_idx = motion_utils::searchZeroVelocityIndex( - points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); - - if (!stop_idx) { - return boost::none; - } - - const auto closest_stop_dist = - motion_utils::calcSignedArcLength(points_with_twist, pose, *stop_idx, max_dist, max_yaw); - - if (!closest_stop_dist) { - return boost::none; - } - - return std::max(0.0, *closest_stop_dist); -} - /** * @brief calculate the point offset from source point index along the trajectory (or path) (points * container) @@ -1617,6 +1490,52 @@ size_t findFirstNearestSegmentIndexWithSoftConstraints( return nearest_idx; } + +/** + * @brief calculate the point offset from source point along the trajectory (or path) + * @brief calculate length of 2D distance between given pose and first point in container with zero + * longitudinal velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @param pose given pose to start the distance calculation from + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * pose + * @return Length of 2D distance between given pose and first point in container with zero + * longitudinal velocity + */ +template +boost::optional calcDistanceToForwardStopPoint( + const T & points_with_twist, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } + + const auto nearest_segment_idx = + motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); + + if (!nearest_segment_idx) { + return boost::none; + } + + const auto stop_idx = motion_utils::searchZeroVelocityIndex( + points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); + + if (!stop_idx) { + return boost::none; + } + + const auto closest_stop_dist = + calcSignedArcLength(points_with_twist, pose.position, *nearest_segment_idx, *stop_idx); + + return std::max(0.0, closest_stop_dist); +} } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 9bdcf1208540a..26d7e2081203b 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -688,48 +688,6 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(4.3, 7.0, 0.0), 2), -2.3, epsilon); } -TEST(trajectory, calcSignedArcLengthFromPoseToIndex_DistThreshold) -{ - using motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Out of threshold - EXPECT_FALSE(calcSignedArcLength(traj.points, createPose(0.0, 0.6, 0.0, 0.0, 0.0, 0.0), 3, 0.5)); - - // On threshold - EXPECT_NEAR( - *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0, - epsilon); - - // Within threshold - EXPECT_NEAR( - *calcSignedArcLength(traj.points, createPose(0.0, 0.4, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0, - epsilon); -} - -TEST(trajectory, calcSignedArcLengthFromPoseToIndex_YawThreshold) -{ - using motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto max_d = std::numeric_limits::max(); - - // Out of threshold - EXPECT_FALSE( - calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.1), 3, max_d, 1.0)); - - // On threshold - EXPECT_NEAR( - *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.0), 3, max_d, 1.0), 3.0, - epsilon); - - // Within threshold - EXPECT_NEAR( - *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.9), 3, max_d, 1.0), 3.0, - epsilon); -} - TEST(trajectory, calcSignedArcLengthFromIndexToPoint) { using motion_utils::calcSignedArcLength; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index ddd919612538f..80856bffb1028 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -390,12 +390,20 @@ BehaviorModuleOutput PullOverModule::plan() // Check if it needs to decide path if (status_.is_safe) { - const auto dist_to_parking_start_pose = calcSignedArcLength( - getCurrentPath().points, current_pose, status_.pull_over_path.start_pose.position, - std::numeric_limits::max(), M_PI_2); + const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + getCurrentPath().points, current_pose, std::numeric_limits::max(), M_PI_2); - if (*dist_to_parking_start_pose < parameters_.decide_path_distance) { - status_.has_decided_path = true; + if (ego_segment_idx) { + const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( + getCurrentPath().points, status_.pull_over_path.start_pose.position); + + const auto dist_to_parking_start_pose = calcSignedArcLength( + getCurrentPath().points, current_pose.position, *ego_segment_idx, + status_.pull_over_path.start_pose.position, start_pose_segment_idx); + + if (dist_to_parking_start_pose < parameters_.decide_path_distance) { + status_.has_decided_path = true; + } } } @@ -608,14 +616,23 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() std::pair PullOverModule::calcDistanceToPathChange() const { const auto & full_path = status_.pull_over_path.getFullPath(); + + const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), + M_PI_2); + + const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( + full_path.points, status_.pull_over_path.start_pose.position); + const auto dist_to_parking_start_pose = calcSignedArcLength( - full_path.points, planner_data_->self_odometry->pose.pose, - status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); + full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, + status_.pull_over_path.start_pose.position, start_pose_segment_idx); + const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, modified_goal_pose_->goal_pose.position); const double start_distance_to_path_change = - dist_to_parking_start_pose ? *dist_to_parking_start_pose : std::numeric_limits::max(); + dist_to_parking_start_pose ? dist_to_parking_start_pose : std::numeric_limits::max(); return {start_distance_to_path_change, dist_to_parking_finish_pose}; } diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index c02f9877b4daa..1c1bbd42b1115 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -133,9 +133,14 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit(current_vel, max_acc, delay_response_time); const auto stop_point_pose = path->points.at(stop_line_idx).point.pose; - const auto distance_until_stop = - motion_utils::calcSignedArcLength(input_path.points, current_pose, stop_point_pose.position); - if (distance_until_stop == boost::none) return true; + const auto ego_segment_idx = + motion_utils::findNearestSegmentIndex(input_path.points, current_pose); + if (ego_segment_idx == boost::none) return true; + const size_t stop_point_segment_idx = + motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); + const auto distance_until_stop = motion_utils::calcSignedArcLength( + input_path.points, current_pose.position, *ego_segment_idx, stop_point_pose.position, + stop_point_segment_idx); /* get debug info */ const auto stop_line_pose = planning_utils::getAheadPose( @@ -152,7 +157,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const double eps = 1e-1; // to prevent hunting if ( current_state == StateMachine::State::GO && - *distance_until_stop + eps < pass_judge_line_dist) { + distance_until_stop + eps < pass_judge_line_dist) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); *path = input_path; // reset path setSafe(true); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index ffdf987b3be12..a37154c20f1d7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -107,10 +107,15 @@ bool OcclusionSpotModule::modifyPathVelocity( //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, path_interpolated, logger_); const geometry_msgs::msg::Point start_point = path_interpolated.points.at(0).point.pose.position; + const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + path_interpolated.points, ego_pose, param_.dist_thr, param_.angle_thr); + if (ego_segment_idx == boost::none) return true; + const size_t start_point_segment_idx = + motion_utils::findNearestSegmentIndex(path_interpolated.points, start_point); const auto offset = motion_utils::calcSignedArcLength( - path_interpolated.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); - if (offset == boost::none) return true; - const double offset_from_start_to_ego = -offset.get(); + path_interpolated.points, ego_pose.position, *ego_segment_idx, start_point, + start_point_segment_idx); + const double offset_from_start_to_ego = -offset; const bool show_time = param_.is_show_processing_time; if (show_time) stop_watch_.tic("processing_time"); PathWithLaneId predicted_path; diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 262e2b619d36e..062407e45a6f7 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -595,18 +595,16 @@ bool OptimizationBasedPlanner::checkOnTrajectory( boost::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose( const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose) { - const auto traj_length = motion_utils::calcSignedArcLength( - traj.points, current_pose, traj.points.size() - 1, ego_nearest_param_.dist_threshold, - ego_nearest_param_.yaw_threshold); + const size_t ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); - if (!traj_length) { - return {}; - } + const auto traj_length = motion_utils::calcSignedArcLength( + traj.points, current_pose.position, ego_segment_idx, traj.points.size() - 1); const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint( traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_closest_stop_point) { - return std::min(traj_length.get(), dist_to_closest_stop_point.get()); + return std::min(traj_length, dist_to_closest_stop_point.get()); } return traj_length; diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 420b72d2bf0aa..4814010f68828 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -143,18 +143,14 @@ Trajectory PlannerInterface::generateStopTrajectory( const double closest_obstacle_dist = motion_utils::calcSignedArcLength( planner_data.traj.points, 0, closest_stop_obstacle->collision_points.front().point); - const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( - planner_data.traj.points, planner_data.current_pose, 0, ego_nearest_param_.dist_threshold, + const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + planner_data.traj.points, planner_data.current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); - if (!negative_dist_to_ego) { - // delete marker - const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker); - return planner_data.traj; - } - const double dist_to_ego = -negative_dist_to_ego.get(); + const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( + planner_data.traj.points, planner_data.current_pose.position, ego_segment_idx, 0); + + const double dist_to_ego = -negative_dist_to_ego; // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin // we set closest_obstacle_stop_distance to closest_behavior_stop_distance @@ -260,15 +256,16 @@ double PlannerInterface::calcDistanceToCollisionPoint( ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); - const auto dist_to_collision_point = motion_utils::calcSignedArcLength( - planner_data.traj.points, planner_data.current_pose, collision_point, - ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); + const size_t ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + planner_data.traj.points, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); - if (dist_to_collision_point) { - return dist_to_collision_point.get() - offset; - } + const size_t collision_segment_idx = + motion_utils::findNearestSegmentIndex(planner_data.traj.points, collision_point); + + const auto dist_to_collision_point = motion_utils::calcSignedArcLength( + planner_data.traj.points, planner_data.current_pose.position, ego_segment_idx, collision_point, + collision_segment_idx); - return motion_utils::calcSignedArcLength( - planner_data.traj.points, planner_data.current_pose.position, collision_point) - - offset; + return dist_to_collision_point - offset; } From 5f8f448517b1f7d0679498516840c09b48840204 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 4 Mar 2023 01:25:46 +0900 Subject: [PATCH 23/24] feat(behavior_path_planner): pull over freespace parking (#2879) * feat(behavior_path_planner): pull over freespace parking Signed-off-by: kosuke55 * Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp Co-authored-by: Takamasa Horibe * fix from review Signed-off-by: kosuke55 * add require_increment_ explanation make the function Signed-off-by: kosuke55 * Update planning/behavior_path_planner/README.md * fix mutex Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 * fix build Signed-off-by: kosuke55 * pre-commit Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 Co-authored-by: Takamasa Horibe --- .../behavior_planning.launch.py | 4 + .../scenario_planning/parking.launch.py | 2 +- planning/behavior_path_planner/CMakeLists.txt | 1 + .../config/pull_over/pull_over.param.yaml | 31 ++ .../behavior_path_planner_node.hpp | 2 + .../behavior_path_planner/data_manager.hpp | 1 + .../behavior_path_planner/path_utilities.hpp | 21 ++ .../pull_over/pull_over_module.hpp | 41 ++- .../util/pull_over/freespace_pull_over.hpp | 60 ++++ .../util/pull_over/pull_over_parameters.hpp | 2 + .../util/pull_over/pull_over_planner_base.hpp | 1 + .../behavior_path_planner/utilities.hpp | 4 + planning/behavior_path_planner/package.xml | 1 + .../src/behavior_path_planner_node.cpp | 10 + .../src/path_utilities.cpp | 153 +++++++++ .../pull_over/pull_over_module.cpp | 292 ++++++++++++++---- .../util/pull_over/freespace_pull_over.cpp | 195 ++++++++++++ .../src/util/pull_over/shift_pull_over.cpp | 57 +--- .../behavior_path_planner/src/utilities.cpp | 137 ++++++++ .../abstract_algorithm.hpp | 25 ++ .../freespace_planning_algorithms/rrtstar.hpp | 2 +- .../freespace_planning_algorithms/package.xml | 1 + 22 files changed, 917 insertions(+), 126 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp create mode 100644 planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 293d8bebf2270..4616590facb06 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -67,6 +67,10 @@ def launch_setup(context, *args, **kwargs): ("~/input/vector_map", LaunchConfiguration("map_topic_name")), ("~/input/perception", "/perception/object_recognition/objects"), ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), + ( + "~/input/costmap", + "/planning/scenario_planning/parking/costmap_generator/occupancy_grid", + ), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/accel", "/localization/acceleration"), ("~/input/scenario", "/planning/scenario_planning/scenario"), diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index dca6866a6c811..d56fecb4f6a0f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -62,7 +62,7 @@ def launch_setup(context, *args, **kwargs): "vehicle_frame": "base_link", "map_frame": "map", "update_rate": 10.0, - "activate_by_scenario": True, + "activate_by_scenario": False, "grid_min_value": 0.0, "grid_max_value": 1.0, "grid_resolution": 0.2, diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index a636b1ea0d766..47f036264ed86 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -22,6 +22,7 @@ set(common_src src/util/pull_over/util.cpp src/util/pull_over/shift_pull_over.cpp src/util/pull_over/geometric_pull_over.cpp + src/util/pull_over/freespace_pull_over.cpp src/util/pull_over/goal_searcher.cpp src/util/pull_out/util.cpp src/util/pull_out/shift_pull_out.cpp diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 5a3d4fb771381..d3a452a0db4f1 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -47,6 +47,37 @@ backward_parking_lane_departure_margin: 0.0 arc_path_interval: 1.0 pull_over_max_steer_angle: 0.35 # 20deg + # freespace parking + enable_freespace_parking: true + freespace_parking: + planning_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 # hazard on when parked hazard_on_threshold_distance: 1.0 hazard_on_threshold_velocity: 0.5 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 55b17e08aed2a..7413beb3c2574 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -111,6 +111,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; + rclcpp::Subscription::SharedPtr costmap_subscriber_; rclcpp::Subscription::SharedPtr operation_mode_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; @@ -171,6 +172,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); + void onCostMap(const OccupancyGrid::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index a644fa955ed23..82b9295c49446 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -123,6 +123,7 @@ struct PlannerData AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{}; PredictedObjects::ConstSharedPtr dynamic_object{}; OccupancyGrid::ConstSharedPtr occupancy_grid{}; + OccupancyGrid::ConstSharedPtr costmap{}; OperationModeState::ConstSharedPtr operation_mode{}; PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp index 0e423dbd46c95..2e417c13c60fe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/util/path_shifter/path_shifter.hpp" #include +#include #include #include @@ -71,6 +72,26 @@ std::pair getPathTurnSignal( const ShiftLine & shift_line, const Pose & pose, const double & velocity, const BehaviorPathPlannerParameters & common_parameter); +PathWithLaneId convertWayPointsToPathWithLaneId( + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + +std::vector getReversingIndices(const PathWithLaneId & path); + +std::vector dividePath( + const PathWithLaneId & path, const std::vector indices); + +void correctDividedPathVelocity(std::vector & divided_paths); + +bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold); + +// only two points is supported +std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s); + +std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval); + } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__PATH_UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 45648ca54e594..24a5b7f27a792 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -18,11 +18,14 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/util/pull_over/freespace_pull_over.hpp" #include "behavior_path_planner/util/pull_over/geometric_pull_over.hpp" #include "behavior_path_planner/util/pull_over/goal_searcher.hpp" #include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp" #include "behavior_path_planner/util/pull_over/shift_pull_over.hpp" +#include +#include #include #include @@ -44,17 +47,27 @@ using nav_msgs::msg::OccupancyGrid; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStar; +using freespace_planning_algorithms::RRTStarParam; + enum PathType { NONE = 0, SHIFT, ARC_FORWARD, ARC_BACKWARD, + FREESPACE, }; struct PUllOverStatus { - PullOverPath pull_over_path{}; + std::shared_ptr pull_over_path{}; + std::shared_ptr lane_parking_pull_over_path{}; size_t current_path_idx{0}; + bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets pull_over_lanes{}; @@ -65,6 +78,8 @@ struct PUllOverStatus bool has_decided_velocity{false}; bool has_requested_approval{false}; std::optional stop_pose{}; + bool is_ready{false}; + bool during_freespace_parking{false}; }; class PullOverModule : public SceneModuleInterface @@ -99,8 +114,8 @@ class PullOverModule : public SceneModuleInterface PullOverParameters parameters_; std::vector> pull_over_planners_; + std::unique_ptr freespace_planner_; std::shared_ptr goal_searcher_; - PullOverPath shift_parking_path_; vehicle_info_util::VehicleInfo vehicle_info_; @@ -116,21 +131,27 @@ class PullOverModule : public SceneModuleInterface std::optional closest_start_pose_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; - std::deque odometry_buffer_; + std::deque odometry_buffer_stopped_; + std::deque odometry_buffer_stuck_; + tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::unique_ptr last_received_time_; std::unique_ptr last_approved_time_; + std::unique_ptr last_increment_time_; std::unique_ptr last_approved_pose_; - void incrementPathIndex(); + bool incrementPathIndex(); PathWithLaneId getCurrentPath() const; Pose calcRefinedGoal(const Pose & goal_pose) const; ParallelParkingParameters getGeometricPullOverParameters() const; std::pair calcDistanceToPathChange() const; PathWithLaneId generateStopPath(); PathWithLaneId generateEmergencyStopPath(); + void keepStoppedWithCurrentPath(PathWithLaneId & path); bool isStopped(); + bool isStopped( + std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); bool hasFinishedPullOver(); void updateOccupancyGrid(); @@ -141,15 +162,23 @@ class PullOverModule : public SceneModuleInterface TurnSignalInfo calcTurnSignalInfo() const; + bool planFreespacePath(); + bool returnToLaneParking(); + bool isStuck(); + // timer for generating pull over path candidates void onTimer(); - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::CallbackGroup::SharedPtr timer_cb_group_; + rclcpp::TimerBase::SharedPtr lane_parking_timer_; + rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; std::mutex mutex_; // debug void setDebugData(); void printParkingPositionError() const; + + void onFreespaceParkingTimer(); + rclcpp::TimerBase::SharedPtr freespace_parking_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp new file mode 100644 index 0000000000000..c08afe5f7b97f --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_ + +#include "behavior_path_planner/util/pull_over/pull_over_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStar; +using freespace_planning_algorithms::RRTStarParam; + +class FreespacePullOver : public PullOverPlannerBase +{ +public: + FreespacePullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info); + + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } + + boost::optional plan(const Pose & goal_pose) override; + +protected: + PlannerCommonParam getCommonParam(rclcpp::Node & node) const; + AstarParam getAstarParam(rclcpp::Node & node) const; + RRTStarParam getRRTStarParam(rclcpp::Node & node) const; + + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp index 612e5c1e3703e..6a78f2ed692d9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp @@ -69,6 +69,8 @@ struct PullOverParameters double backward_parking_lane_departure_margin; double arc_path_interval; double pull_over_max_steer_angle; + // freespace parking + bool enable_freespace_parking; // hazard double hazard_on_threshold_distance; double hazard_on_threshold_velocity; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp index abf2e796a2bb4..e15a58b8eda37 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp @@ -39,6 +39,7 @@ enum class PullOverPlannerType { SHIFT, ARC_FORWARD, ARC_BACKWARD, + FREESPACE, }; struct PullOverPath diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 89f1beedefd6d..f55dfe08c2de0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -333,6 +333,10 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, const std::shared_ptr planner_data, const bool is_driving_forward = true); +void generateDrivableArea( + PathWithLaneId & path, const double vehicle_length, const double vehicle_width, + const double margin, const bool is_driving_forward = true); + lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 1b50220431362..d96610d775ffc 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -47,6 +47,7 @@ autoware_auto_tf2 autoware_auto_vehicle_msgs behaviortree_cpp_v3 + freespace_planning_algorithms geometry_msgs interpolation lane_departure_checker diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 84d0b4a844c32..c876d6b40f2b6 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -91,6 +91,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod occupancy_grid_subscriber_ = create_subscription( "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); + costmap_subscriber_ = create_subscription( + "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), + createSubscriptionOptions(this)); operation_mode_subscriber_ = create_subscription( "/system/operation_mode/state", 1, std::bind(&BehaviorPathPlannerNode::onOperationMode, this, _1), @@ -633,6 +636,8 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.backward_parking_lane_departure_margin = dp("backward_parking_lane_departure_margin", 0.0); p.arc_path_interval = dp("arc_path_interval", 1.0); p.pull_over_max_steer_angle = dp("pull_over_max_steer_angle", 0.35); // 20deg + // freespace parking + p.enable_freespace_parking = dp("enable_freespace_parking", true); // hazard p.hazard_on_threshold_distance = dp("hazard_on_threshold_distance", 1.0); p.hazard_on_threshold_velocity = dp("hazard_on_threshold_velocity", 0.5); @@ -1160,6 +1165,11 @@ void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPt const std::lock_guard lock(mutex_pd_); planner_data_->occupancy_grid = msg; } +void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) +{ + const std::lock_guard lock(mutex_pd_); + planner_data_->costmap = msg; +} void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_pd_); diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index a2a3343986576..3f01b7596bb80 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -313,4 +313,157 @@ std::pair getPathTurnSignal( return std::make_pair(turn_signal, max_distance); } +PathWithLaneId convertWayPointsToPathWithLaneId( + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) +{ + PathWithLaneId path; + path.header = waypoints.header; + for (const auto & waypoint : waypoints.waypoints) { + PathPointWithLaneId point{}; + point.point.pose = waypoint.pose.pose; + // point.lane_id = // todo + point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; + path.points.push_back(point); + } + return path; +} + +std::vector getReversingIndices(const PathWithLaneId & path) +{ + std::vector indices; + + for (size_t i = 0; i < path.points.size() - 1; ++i) { + if ( + path.points.at(i).point.longitudinal_velocity_mps * + path.points.at(i + 1).point.longitudinal_velocity_mps < + 0) { + indices.push_back(i); + } + } + + return indices; +} + +std::vector dividePath( + const PathWithLaneId & path, const std::vector indices) +{ + std::vector divided_paths; + + if (indices.empty()) { + divided_paths.push_back(path); + return divided_paths; + } + + for (size_t i = 0; i < indices.size(); ++i) { + PathWithLaneId divided_path; + divided_path.header = path.header; + if (i == 0) { + divided_path.points.insert( + divided_path.points.end(), path.points.begin(), path.points.begin() + indices.at(i) + 1); + } else { + // include the point at indices.at(i - 1) and indices.at(i) + divided_path.points.insert( + divided_path.points.end(), path.points.begin() + indices.at(i - 1), + path.points.begin() + indices.at(i) + 1); + } + divided_paths.push_back(divided_path); + } + + PathWithLaneId divided_path; + divided_path.header = path.header; + divided_path.points.insert( + divided_path.points.end(), path.points.begin() + indices.back(), path.points.end()); + divided_paths.push_back(divided_path); + + return divided_paths; +} + +void correctDividedPathVelocity(std::vector & divided_paths) +{ + for (auto & path : divided_paths) { + const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + if (!is_driving_forward) { + continue; + } + + if (*is_driving_forward) { + for (auto & point : path.points) { + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + } else { + for (auto & point : path.points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + } + path.points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold) +{ + for (const auto & point : path.points) { + const auto & p = point.point.pose.position; + const double dist = std::hypot(pose.position.x - p.x, pose.position.y - p.y); + if (dist < distance_threshold) { + return true; + } + } + return false; +} + +// only two points is supported +std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s) +{ + const double h = base_s.at(1) - base_s.at(0); + + const double c = begin_diff; + const double d = base_x.at(0); + const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); + const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); + + std::vector res; + for (const auto & s : new_s) { + const double ds = s - base_s.at(0); + res.push_back(d + (c + (b + a * ds) * ds) * ds); + } + + return res; +} + +std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval) +{ + std::vector interpolated_poses{}; // output + + const std::vector base_s{ + 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const std::vector base_x{start_pose.position.x, end_pose.position.x}; + const std::vector base_y{start_pose.position.y, end_pose.position.y}; + std::vector new_s; + + constexpr double eps = 0.3; // prevent overlapping + for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + new_s.push_back(s); + } + + const std::vector interpolated_x = splineTwoPoints( + base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), + std::cos(tf2::getYaw(end_pose.orientation)), new_s); + const std::vector interpolated_y = splineTwoPoints( + base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), + std::sin(tf2::getYaw(end_pose.orientation)), new_s); + for (size_t i = 0; i < interpolated_x.size(); ++i) { + Pose pose{}; + pose = util::lerpByPose(end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + pose.position.x = interpolated_x.at(i); + pose.position.y = interpolated_y.at(i); + pose.position.z = end_pose.position.z; + interpolated_poses.push_back(pose); + } + + return interpolated_poses; +} + } // namespace behavior_path_planner::util diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 80856bffb1028..cdcff55327c99 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -90,11 +90,24 @@ PullOverModule::PullOverModule( // for collision check with objects vehicle_footprint_ = createVehicleFootprint(vehicle_info_); - // timer callback for generating candidate paths - const auto period_ns = rclcpp::Rate(1.0).period(); - timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - timer_ = rclcpp::create_timer( - &node, clock_, period_ns, std::bind(&PullOverModule::onTimer, this), timer_cb_group_); + // timer callback for generating lane parking candidate paths + const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); + lane_parking_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + lane_parking_timer_ = rclcpp::create_timer( + &node, clock_, lane_parking_period_ns, std::bind(&PullOverModule::onTimer, this), + lane_parking_timer_cb_group_); + + // freespace parking + if (parameters_.enable_freespace_parking) { + freespace_planner_ = std::make_unique(node, parameters, vehicle_info_); + const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); + freespace_parking_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + freespace_parking_timer_ = rclcpp::create_timer( + &node, clock_, freespace_parking_period_ns, + std::bind(&PullOverModule::onFreespaceParkingTimer, this), freespace_parking_timer_cb_group_); + } resetStatus(); } @@ -130,6 +143,9 @@ void PullOverModule::onTimer() if (goal_candidates_.empty()) { return; } + mutex_.lock(); + const auto goal_candidates = goal_candidates_; + mutex_.unlock(); // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); @@ -158,12 +174,12 @@ void PullOverModule::onTimer() // plan candidate paths and set them to the member variable if (parameters_.search_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { - for (const auto & goal_candidate : goal_candidates_) { + for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } } } else if (parameters_.search_priority == "close_goal") { - for (const auto & goal_candidate : goal_candidates_) { + for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { planCandidatePaths(planner, goal_candidate); } @@ -182,6 +198,22 @@ void PullOverModule::onTimer() mutex_.unlock(); } +void PullOverModule::onFreespaceParkingTimer() +{ + if (!planner_data_) { + return; + } + if (!planner_data_->costmap) { + return; + } + + const bool is_new_costmap = + (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + if (isStuck() && is_new_costmap) { + planFreespacePath(); + } +} + BehaviorModuleOutput PullOverModule::run() { current_state_ = BT::NodeStatus::RUNNING; @@ -252,8 +284,6 @@ void PullOverModule::onEntry() } last_received_time_ = std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); - - // Use refined goal as modified goal when disabling goal research } void PullOverModule::onExit() @@ -377,6 +407,62 @@ BT::NodeStatus PullOverModule::updateState() return current_state_; } +bool PullOverModule::planFreespacePath() +{ + mutex_.lock(); + goal_searcher_->update(goal_candidates_); + const auto goal_candidates = goal_candidates_; + mutex_.unlock(); + + for (const auto & goal_candidate : goal_candidates) { + if (!goal_candidate.is_safe) { + continue; + } + freespace_planner_->setPlannerData(planner_data_); + auto freespace_path = freespace_planner_->plan(goal_candidate.goal_pose); + freespace_path->goal_id = goal_candidate.id; + if (!freespace_path) { + continue; + } + mutex_.lock(); + status_.pull_over_path = std::make_shared(*freespace_path); + status_.current_path_idx = 0; + status_.is_safe = true; + modified_goal_pose_ = goal_candidate; + mutex_.unlock(); + return true; + } + return false; +} + +bool PullOverModule::returnToLaneParking() +{ + if (!status_.lane_parking_pull_over_path) { + return false; + } + + const PathWithLaneId path = status_.lane_parking_pull_over_path->getFullPath(); + if (checkCollision(path)) { + return false; + } + + const Point & current_point = planner_data_->self_odometry->pose.pose.position; + constexpr double th_distance = 0.5; + const bool is_close_to_path = + std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; + if (!is_close_to_path) { + return false; + } + + mutex_.lock(); + status_.has_decided_path = false; + status_.pull_over_path = status_.lane_parking_pull_over_path; + status_.current_path_idx = 0; + mutex_.unlock(); + + return true; +} + BehaviorModuleOutput PullOverModule::plan() { const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -392,15 +478,12 @@ BehaviorModuleOutput PullOverModule::plan() if (status_.is_safe) { const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( getCurrentPath().points, current_pose, std::numeric_limits::max(), M_PI_2); - if (ego_segment_idx) { const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, status_.pull_over_path.start_pose.position); - + getCurrentPath().points, status_.pull_over_path->start_pose.position); const auto dist_to_parking_start_pose = calcSignedArcLength( getCurrentPath().points, current_pose.position, *ego_segment_idx, - status_.pull_over_path.start_pose.position, start_pose_segment_idx); - + status_.pull_over_path->start_pose.position, start_pose_segment_idx); if (dist_to_parking_start_pose < parameters_.decide_path_distance) { status_.has_decided_path = true; } @@ -425,7 +508,7 @@ BehaviorModuleOutput PullOverModule::plan() // decide velocity to guarantee turn signal lighting time if (!status_.has_decided_velocity) { - auto & first_path = status_.pull_over_path.partial_paths.front(); + auto & first_path = status_.pull_over_path->partial_paths.front(); const auto vel = static_cast(std::max( planner_data_->self_odometry->twist.twist.linear.x, parameters_.pull_over_minimum_velocity)); @@ -442,11 +525,13 @@ BehaviorModuleOutput PullOverModule::plan() const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > planner_data_->parameters.turn_signal_search_time; - if (hasFinishedCurrentPath() && has_passed_enough_time) { - incrementPathIndex(); + if (hasFinishedCurrentPath() && has_passed_enough_time && status_.require_increment_) { + if (incrementPathIndex()) { + last_increment_time_ = std::make_unique(clock_->now()); + } } } - } else { + } else if (!pull_over_path_candidates_.empty()) { // select safe path from pull over path candidates goal_searcher_->setPlannerData(planner_data_); mutex_.lock(); @@ -472,18 +557,22 @@ BehaviorModuleOutput PullOverModule::plan() } status_.is_safe = true; - status_.pull_over_path = pull_over_path; + mutex_.lock(); + status_.pull_over_path = std::make_shared(pull_over_path); + status_.current_path_idx = 0; + status_.lane_parking_pull_over_path = status_.pull_over_path; modified_goal_pose_ = *goal_candidate_it; + mutex_.unlock(); break; } // decelerate before the search area start if (status_.is_safe) { const auto search_start_pose = calcLongitudinalOffsetPose( - status_.pull_over_path.getFullPath().points, refined_goal_pose_.position, + status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); if (search_start_pose) { - auto & first_path = status_.pull_over_path.partial_paths.front(); + auto & first_path = status_.pull_over_path->partial_paths.front(); constexpr double deceleration_buffer = 15.0; first_path = util::setDecelerationVelocity( first_path, parameters_.pull_over_velocity, *search_start_pose, -deceleration_buffer, @@ -492,7 +581,7 @@ BehaviorModuleOutput PullOverModule::plan() } // generate drivable area for each partial path - for (auto & path : status_.pull_over_path.partial_paths) { + for (auto & path : status_.pull_over_path->partial_paths) { const size_t ego_idx = planner_data_->findEgoIndex(path.points); util::clipPathLength(path, ego_idx, planner_data_->parameters); const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); @@ -508,8 +597,14 @@ BehaviorModuleOutput PullOverModule::plan() if (status_.is_safe) { // safe: use pull over path status_.stop_pose.reset(); - output.path = std::make_shared(getCurrentPath()); - path_candidate_ = std::make_shared(status_.pull_over_path.getFullPath()); + + // keep stop if not enough time passed, + // because it takes time for the trajectory to be reflected + auto current_path = getCurrentPath(); + keepStoppedWithCurrentPath(current_path); + + output.path = std::make_shared(current_path); + path_candidate_ = std::make_shared(status_.pull_over_path->getFullPath()); } else { // not safe: use stop_path if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { @@ -518,8 +613,11 @@ BehaviorModuleOutput PullOverModule::plan() status_.prev_stop_path = output.path; // set stop path as pull over path PullOverPath pull_over_path{}; - status_.pull_over_path = pull_over_path; - status_.pull_over_path.partial_paths.push_back(*output.path); + mutex_.lock(); + status_.pull_over_path = std::make_shared(pull_over_path); + status_.current_path_idx = 0; + status_.pull_over_path->partial_paths.push_back(*output.path); + mutex_.unlock(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { @@ -531,6 +629,16 @@ BehaviorModuleOutput PullOverModule::plan() } status_.prev_is_safe = status_.is_safe; + // return to lane parking if it is possible + if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + // return only before starting free space parking + if (!isStopped() || status_.during_freespace_parking) { + status_.during_freespace_parking = true; + } else if (returnToLaneParking()) { + RCLCPP_INFO(getLogger(), "return to lane parking"); + } + } + // set hazard and turn signal if (status_.has_decided_path) { output.turn_signal_info = calcTurnSignalInfo(); @@ -551,6 +659,8 @@ BehaviorModuleOutput PullOverModule::plan() modified_goal.header = planner_data_->route_handler->getRouteHeader(); output.modified_goal = modified_goal; prev_goal_id_ = modified_goal_pose_->id; + } else { + output.modified_goal = {}; } const uint16_t steering_factor_direction = std::invoke([this]() { @@ -565,7 +675,7 @@ BehaviorModuleOutput PullOverModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_over_path.start_pose, modified_goal_pose_->goal_pose}, + {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, steering_factor_direction, SteeringFactor::TURNING, ""); @@ -589,7 +699,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); path_candidate_ = status_.is_safe - ? std::make_shared(status_.pull_over_path.getFullPath()) + ? std::make_shared(status_.pull_over_path->getFullPath()) : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); @@ -605,7 +715,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() }); steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_over_path.start_pose, modified_goal_pose_->goal_pose}, + {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, steering_factor_direction, SteeringFactor::APPROACHING, ""); waitApproval(); @@ -615,25 +725,27 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() std::pair PullOverModule::calcDistanceToPathChange() const { - const auto & full_path = status_.pull_over_path.getFullPath(); + const auto & full_path = status_.pull_over_path->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), M_PI_2); + if (!ego_segment_idx) { + return {std::numeric_limits::max(), std::numeric_limits::max()}; + } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.pull_over_path.start_pose.position); - - const auto dist_to_parking_start_pose = calcSignedArcLength( + full_path.points, status_.pull_over_path->start_pose.position); + const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.pull_over_path.start_pose.position, start_pose_segment_idx); - + status_.pull_over_path->start_pose.position, start_pose_segment_idx); + const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( + full_path.points, modified_goal_pose_->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( - full_path.points, planner_data_->self_odometry->pose.pose.position, - modified_goal_pose_->goal_pose.position); - const double start_distance_to_path_change = - dist_to_parking_start_pose ? dist_to_parking_start_pose : std::numeric_limits::max(); - return {start_distance_to_path_change, dist_to_parking_finish_pose}; + full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, + modified_goal_pose_->goal_pose.position, goal_pose_segment_idx); + + return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } void PullOverModule::setParameters(const PullOverParameters & parameters) @@ -674,7 +786,7 @@ PathWithLaneId PullOverModule::generateStopPath() return generateEmergencyStopPath(); } const Pose stop_pose = - status_.is_safe ? status_.pull_over_path.start_pose + status_.is_safe ? status_.pull_over_path->start_pose : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible @@ -770,31 +882,35 @@ PathWithLaneId PullOverModule::generateEmergencyStopPath() return stop_path; } -void PullOverModule::incrementPathIndex() +bool PullOverModule::incrementPathIndex() { - status_.current_path_idx = - std::min(status_.current_path_idx + 1, status_.pull_over_path.partial_paths.size() - 1); + if (status_.current_path_idx == status_.pull_over_path->partial_paths.size() - 1) { + return false; + } + status_.current_path_idx = status_.current_path_idx + 1; + return true; } PathWithLaneId PullOverModule::getCurrentPath() const { - return status_.pull_over_path.partial_paths.at(status_.current_path_idx); + return status_.pull_over_path->partial_paths.at(status_.current_path_idx); } -bool PullOverModule::isStopped() +bool PullOverModule::isStopped( + std::deque & odometry_buffer, const double time) { - odometry_buffer_.push_back(planner_data_->self_odometry); + odometry_buffer.push_back(planner_data_->self_odometry); // Delete old data in buffer while (rclcpp::ok()) { - const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - - rclcpp::Time(odometry_buffer_.front()->header.stamp); - if (time_diff.seconds() < parameters_.th_stopped_time) { + const auto time_diff = rclcpp::Time(odometry_buffer.back()->header.stamp) - + rclcpp::Time(odometry_buffer.front()->header.stamp); + if (time_diff.seconds() < time) { break; } - odometry_buffer_.pop_front(); + odometry_buffer.pop_front(); } bool is_stopped = true; - for (const auto & odometry : odometry_buffer_) { + for (const auto & odometry : odometry_buffer) { const double ego_vel = util::l2Norm(odometry->twist.twist.linear); if (ego_vel > parameters_.th_stopped_velocity) { is_stopped = false; @@ -804,6 +920,20 @@ bool PullOverModule::isStopped() return is_stopped; } +bool PullOverModule::isStopped() +{ + return isStopped(odometry_buffer_stopped_, parameters_.th_stopped_time); +} + +bool PullOverModule::isStuck() +{ + if (!status_.pull_over_path) { + return false; + } + constexpr double stuck_time = 5.0; + return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); +} + bool PullOverModule::hasFinishedCurrentPath() { const auto & current_path_end = getCurrentPath().points.back(); @@ -829,9 +959,9 @@ TurnSignalInfo PullOverModule::calcTurnSignalInfo() const TurnSignalInfo turn_signal{}; // output const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = status_.pull_over_path.start_pose; - const auto & end_pose = status_.pull_over_path.end_pose; - const auto full_path = status_.pull_over_path.getFullPath(); + const auto & start_pose = status_.pull_over_path->start_pose; + const auto & end_pose = status_.pull_over_path->end_pose; + const auto full_path = status_.pull_over_path->getFullPath(); // calc TurnIndicatorsCommand { @@ -888,27 +1018,45 @@ void PullOverModule::setDebugData() // Visualize path and related pose if (status_.is_safe) { add(createPoseMarkerArray( - status_.pull_over_path.start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add(createPathMarkerArray(status_.pull_over_path.getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + status_.pull_over_path->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add( + createPathMarkerArray(status_.pull_over_path->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + add(createPathMarkerArray(getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); + + // visualize each partial path + for (size_t i = 0; i < status_.pull_over_path->partial_paths.size(); ++i) { + const auto & partial_path = status_.pull_over_path->partial_paths.at(i); + add( + createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); + } } // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; + const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), - createMarkerColor(1.0, 1.0, 1.0, 0.99)); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = modified_goal_pose_->goal_pose; - marker.text = magic_enum::enum_name(status_.pull_over_path.type); + marker.text = magic_enum::enum_name(status_.pull_over_path->type); + marker.text += " " + std::to_string(status_.current_path_idx) + "/" + + std::to_string(status_.pull_over_path->partial_paths.size() - 1); + if (isStuck()) { + marker.text += " stuck"; + } else if (isStopped()) { + marker.text += " stopped"; + } + planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } // Visualize debug poses - const auto & debug_poses = status_.pull_over_path.debug_poses; + const auto & debug_poses = status_.pull_over_path->debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); @@ -969,6 +1117,26 @@ bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) cons return true; } +void PullOverModule::keepStoppedWithCurrentPath(PathWithLaneId & path) +{ + constexpr double keep_stop_time = 2.0; + constexpr double keep_current_idx_buffer_time = 2.0; + if (last_increment_time_) { + const auto time_diff = (clock_->now() - *last_increment_time_).seconds(); + if (time_diff < keep_stop_time) { + status_.require_increment_ = false; + for (auto & p : path.points) { + p.point.longitudinal_velocity_mps = 0.0; + } + } else if (time_diff > keep_stop_time + keep_current_idx_buffer_time) { + // require increment only when the time passed is enough + // to prevent increment before driving + // when the end of the current path is close to the current pose + status_.require_increment_ = true; + } + } +} + void PullOverModule::printParkingPositionError() const { const auto current_pose = planner_data_->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp b/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp new file mode 100644 index 0000000000000..fc433f9f63213 --- /dev/null +++ b/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp @@ -0,0 +1,195 @@ +// Copyright 2023 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 "behavior_path_planner/util/pull_over/freespace_pull_over.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/util/pull_over/util.hpp" + +#include +#include + +namespace behavior_path_planner +{ +FreespacePullOver::FreespacePullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PullOverPlannerBase{node, parameters} +{ + velocity_ = node.declare_parameter("pull_over.freespace_parking.velocity", 1.0); + + const double vehicle_shape_margin = + node.declare_parameter("pull_over.freespace_parking.vehicle_shape_margin", 1.0); + freespace_planning_algorithms::VehicleShape vehicle_shape(vehicle_info, vehicle_shape_margin); + + const auto algorithm = + node.declare_parameter("pull_over.freespace_parking.planning_algorithm", "astar"); + if (algorithm == "astar") { + const auto astar_param = getAstarParam(node); + use_back_ = astar_param.use_back; + planner_ = std::make_unique(getCommonParam(node), vehicle_shape, astar_param); + } else if (algorithm == "rrtstar") { + use_back_ = true; // no option for disabling back in rrtstar + planner_ = + std::make_unique(getCommonParam(node), vehicle_shape, getRRTStarParam(node)); + } +} + +PlannerCommonParam FreespacePullOver::getCommonParam(rclcpp::Node & node) const +{ + const auto dp = [&node](const std::string & str, auto def_val) { + std::string name = "pull_over.freespace_parking." + str; + return node.declare_parameter(name, def_val); + }; + + PlannerCommonParam p{}; + + // search configs + p.time_limit = dp("time_limit", 5000.0); + p.minimum_turning_radius = dp("minimum_turning_radius", 0.5); + p.maximum_turning_radius = dp("maximum_turning_radius", 6.0); + p.turning_radius_size = dp("turning_radius_size", 11); + p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); + p.turning_radius_size = std::max(p.turning_radius_size, 1); + + p.theta_size = dp("theta_size", 48); + p.angle_goal_range = dp("angle_goal_range", 6.0); + + p.curve_weight = dp("curve_weight", 1.2); + p.reverse_weight = dp("reverse_weight", 2.00); + p.lateral_goal_range = dp("lateral_goal_range", 0.5); + p.longitudinal_goal_range = dp("longitudinal_goal_range", 2.0); + + // costmap configs + p.obstacle_threshold = dp("obstacle_threshold", 100); + + return p; +} + +AstarParam FreespacePullOver::getAstarParam(rclcpp::Node & node) const +{ + const auto dp = [&node](const std::string & str, auto def_val) { + std::string name = "pull_over.freespace_parking.astar." + str; + return node.declare_parameter(name, def_val); + }; + + AstarParam p{}; + + p.only_behind_solutions = dp("only_behind_solutions", false); + p.use_back = dp("use_back", true); + p.distance_heuristic_weight = dp("distance_heuristic_weight", 1.0); + + return p; +} + +RRTStarParam FreespacePullOver::getRRTStarParam(rclcpp::Node & node) const +{ + const auto dp = [&node](const std::string & str, auto def_val) { + std::string name = "pull_over.freespace_parking.rrtstar." + str; + return node.declare_parameter(name, def_val); + }; + + RRTStarParam p; + + p.enable_update = dp("enable_update", true); + p.use_informed_sampling = dp("use_informed_sampling", true); + p.max_planning_time = dp("max_planning_time", 150.0); + p.neighbor_radius = dp("neighbor_radius", 8.0); + p.margin = dp("margin", 0.1); + + return p; +} + +boost::optional FreespacePullOver::plan(const Pose & goal_pose) +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + planner_->setMap(*planner_data_->costmap); + + // offset goal pose to make straight path near goal for improving parking precision + // todo: support straight path when using back + constexpr double straight_distance = 1.0; + const Pose end_pose = + use_back_ ? goal_pose + : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); + const bool found_path = planner_->makePlan(current_pose, end_pose); + if (!found_path) { + return {}; + } + + PathWithLaneId path = util::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + const auto reverse_indices = util::getReversingIndices(path); + std::vector partial_paths = util::dividePath(path, reverse_indices); + + // remove points which are near the goal + PathWithLaneId & last_path = partial_paths.back(); + const double th_goal_distance = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_goal_distance) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // add PathPointWithLaneId to last path + auto addPose = [&last_path](const Pose & pose) { + PathPointWithLaneId p = last_path.points.back(); + p.point.pose = pose; + last_path.points.push_back(p); + }; + + if (use_back_) { + addPose(end_pose); + } else { + // add interpolated poses + auto addInterpolatedPoses = [&addPose](const Pose & pose1, const Pose & pose2) { + constexpr double interval = 0.5; + std::vector interpolated_poses = util::interpolatePose(pose1, pose2, interval); + for (const auto & pose : interpolated_poses) { + addPose(pose); + } + }; + addInterpolatedPoses(last_path.points.back().point.pose, end_pose); + addPose(end_pose); + addInterpolatedPoses(end_pose, goal_pose); + addPose(goal_pose); + } + + util::correctDividedPathVelocity(partial_paths); + + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + for (auto & path : partial_paths) { + const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + if (!is_driving_forward) { + // path points is less than 2 + return {}; + } + util::generateDrivableArea( + path, planner_data_->parameters.vehicle_length, planner_data_->parameters.vehicle_width, + drivable_area_margin, *is_driving_forward); + } + + PullOverPath pull_over_path{}; + pull_over_path.partial_paths = partial_paths; + pull_over_path.start_pose = current_pose; + pull_over_path.end_pose = goal_pose; + pull_over_path.type = getPlannerType(); + + return pull_over_path; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/util/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/util/pull_over/shift_pull_over.cpp index af16311e706c8..6ba2ae6c52145 100644 --- a/planning/behavior_path_planner/src/util/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/util/pull_over/shift_pull_over.cpp @@ -143,7 +143,7 @@ boost::optional ShiftPullOver::generatePullOverPath( // interpolate between shift end pose to goal pose std::vector interpolated_poses = - interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); + util::interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); for (const auto & pose : interpolated_poses) { PathPointWithLaneId p = shifted_path.path.points.back(); p.point.pose = pose; @@ -242,59 +242,4 @@ double ShiftPullOver::calcBeforeShiftedArcLength( return before_arc_length; } - -// only two points is supported -std::vector ShiftPullOver::splineTwoPoints( - std::vector base_s, std::vector base_x, const double begin_diff, - const double end_diff, std::vector new_s) -{ - const double h = base_s.at(1) - base_s.at(0); - - const double c = begin_diff; - const double d = base_x.at(0); - const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); - const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); - - std::vector res; - for (const auto & s : new_s) { - const double ds = s - base_s.at(0); - res.push_back(d + (c + (b + a * ds) * ds) * ds); - } - - return res; -} - -std::vector ShiftPullOver::interpolatePose( - const Pose & start_pose, const Pose & end_pose, const double resample_interval) -{ - std::vector interpolated_poses{}; // output - - const std::vector base_s{ - 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; - const std::vector base_x{start_pose.position.x, end_pose.position.x}; - const std::vector base_y{start_pose.position.y, end_pose.position.y}; - std::vector new_s; - - constexpr double eps = 0.3; // prevent overlapping - for (double s = eps; s < base_s.back() - eps; s += resample_interval) { - new_s.push_back(s); - } - - const std::vector interpolated_x = splineTwoPoints( - base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), - std::cos(tf2::getYaw(end_pose.orientation)), new_s); - const std::vector interpolated_y = splineTwoPoints( - base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), - std::sin(tf2::getYaw(end_pose.orientation)), new_s); - for (size_t i = 0; i < interpolated_x.size(); ++i) { - Pose pose{}; - pose = util::lerpByPose(end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); - pose.position.x = interpolated_x.at(i); - pose.position.y = interpolated_y.at(i); - pose.position.z = end_pose.position.z; - interpolated_poses.push_back(pose); - } - - return interpolated_poses; -} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index d34ca2ab819ee..1b5bf239c4153 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1228,6 +1228,143 @@ void generateDrivableArea( } } +// generate drivable area by expanding path for freespace +void generateDrivableArea( + PathWithLaneId & path, const double vehicle_length, const double vehicle_width, + const double margin, const bool is_driving_forward) +{ + using tier4_autoware_utils::calcOffsetPose; + + // remove path points which is close to the previous point + PathWithLaneId resampled_path{}; + const double resample_interval = 2.0; + for (size_t i = 0; i < path.points.size(); ++i) { + if (i == 0) { + resampled_path.points.push_back(path.points.at(i)); + } else { + const auto & prev_point = resampled_path.points.back().point.pose.position; + const auto & curr_point = path.points.at(i).point.pose.position; + const double signed_arc_length = + motion_utils::calcSignedArcLength(path.points, prev_point, curr_point); + if (signed_arc_length > resample_interval) { + resampled_path.points.push_back(path.points.at(i)); + } + } + } + // add last point of path if enough far from the one of resampled path + constexpr double th_last_point_distance = 0.3; + if ( + tier4_autoware_utils::calcDistance2d( + resampled_path.points.back().point.pose.position, path.points.back().point.pose.position) > + th_last_point_distance) { + resampled_path.points.push_back(path.points.back()); + } + + // create bound point by calculating offset point + std::vector left_bound; + std::vector right_bound; + for (const auto & point : resampled_path.points) { + const auto & pose = point.point.pose; + + const auto left_point = calcOffsetPose(pose, 0, vehicle_width / 2.0 + margin, 0); + const auto right_point = calcOffsetPose(pose, 0, -vehicle_width / 2.0 - margin, 0); + + left_bound.push_back(left_point.position); + right_bound.push_back(right_point.position); + } + + if (is_driving_forward) { + // add backward offset point to bound + const Pose first_point = + calcOffsetPose(resampled_path.points.front().point.pose, -vehicle_length, 0, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.insert(left_bound.begin(), left_first_point.position); + right_bound.insert(right_bound.begin(), right_first_point.position); + + // add forward offset point to bound + const Pose last_point = + calcOffsetPose(resampled_path.points.back().point.pose, vehicle_length, 0, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.push_back(left_last_point.position); + right_bound.push_back(right_last_point.position); + } else { + // add forward offset point to bound + const Pose first_point = + calcOffsetPose(resampled_path.points.front().point.pose, vehicle_length, 0, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.insert(left_bound.begin(), left_first_point.position); + right_bound.insert(right_bound.begin(), right_first_point.position); + + // add backward offset point to bound + const Pose last_point = + calcOffsetPose(resampled_path.points.back().point.pose, -vehicle_length, 0, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.push_back(left_last_point.position); + right_bound.push_back(right_last_point.position); + } + + if (left_bound.empty() || right_bound.empty()) { + return; + } + + // fix intersected bound + // if bound is intersected, remove them and insert intersection point + typedef boost::geometry::model::d2::point_xy BoostPoint; + typedef boost::geometry::model::linestring LineString; + auto modify_bound_intersection = [](const std::vector & bound) { + const double intersection_check_distance = 10.0; + std::vector modified_bound; + size_t i = 0; + while (i < bound.size() - 1) { + BoostPoint p1(bound.at(i).x, bound.at(i).y); + BoostPoint p2(bound.at(i + 1).x, bound.at(i + 1).y); + LineString p_line; + p_line.push_back(p1); + p_line.push_back(p2); + bool intersection_found = false; + for (size_t j = i + 2; j < bound.size() - 1; j++) { + const double distance = tier4_autoware_utils::calcDistance2d(bound.at(i), bound.at(j)); + if (distance > intersection_check_distance) { + break; + } + LineString q_line; + BoostPoint q1(bound.at(j).x, bound.at(j).y); + BoostPoint q2(bound.at(j + 1).x, bound.at(j + 1).y); + q_line.push_back(q1); + q_line.push_back(q2); + std::vector intersection_points; + boost::geometry::intersection(p_line, q_line, intersection_points); + if (intersection_points.size() > 0) { + modified_bound.push_back(bound.at(i)); + Point intersection_point; + intersection_point.x = intersection_points.at(0).x(); + intersection_point.y = intersection_points.at(0).y(); + modified_bound.push_back(intersection_point); + i = j + 1; + intersection_found = true; + break; + } + } + if (!intersection_found) { + modified_bound.push_back(bound.at(i)); + i++; + } + } + modified_bound.push_back(bound.back()); + return modified_bound; + }; + std::vector modified_left_bound = modify_bound_intersection(left_bound); + std::vector modified_right_bound = modify_bound_intersection(right_bound); + + // set bound to path + path.left_bound = modified_left_bound; + path.right_bound = modified_right_bound; +} + double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index f9ecdbc826d5d..1034075a9bec5 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -16,6 +16,7 @@ #define FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ #include +#include #include #include @@ -59,6 +60,23 @@ struct VehicleShape double length; // X [m] double width; // Y [m] double base2back; // base_link to rear [m] + + VehicleShape() = default; + + VehicleShape(double length, double width, double base2back) + { + length = length; + width = width; + base2back = base2back; + } + + explicit VehicleShape( + const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + { + length = vehicle_info.vehicle_length_m + margin; + width = vehicle_info.vehicle_width_m + margin; + base2back = vehicle_info.rear_overhang_m + margin / 2.0; + } }; struct PlannerCommonParam @@ -106,6 +124,13 @@ class AbstractPlanningAlgorithm { } + AbstractPlanningAlgorithm( + const PlannerCommonParam & planner_common_param, + const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin) + { + } + virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap); virtual bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp index eb091b9066bff..07f73281e7176 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp @@ -50,7 +50,7 @@ class RRTStar : public AbstractPlanningAlgorithm node.declare_parameter("rrtstar.enable_update", true), node.declare_parameter("rrtstar.use_informed_sampling", true), node.declare_parameter("rrtstar.max_planning_time", 150.0), - node.declare_parameter("rrtstar.neighbour_radius", 8.0), + node.declare_parameter("rrtstar.neighbor_radius", 8.0), node.declare_parameter("rrtstar.margin", 0.1)}) { } diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 4fc302d58de65..6ba150bb48d92 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -25,6 +25,7 @@ tf2 tf2_geometry_msgs tier4_autoware_utils + vehicle_info_util ament_cmake_ros ament_lint_auto From 806db52c35343f53fc843142e9af262db9fe554a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 4 Mar 2023 01:26:34 +0900 Subject: [PATCH 24/24] docs(behavior_path_planner): add pull over out separated docs and update them for freespace (#2948) * docs(behavior_path_planner): add pull over out separated docs and update them for freespace Signed-off-by: kosuke55 * Update planning/behavior_path_planner/behavior_path_planner_pull_over-design.md Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/behavior_path_planner_pull_over-design.md Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/behavior_path_planner_pull_over-design.md Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/behavior_path_planner_pull_out-design.md Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix typo Signed-off-by: kosuke55 * pre-commit Signed-off-by: kosuke55 * pre-comit Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- planning/behavior_path_planner/README.md | 162 +--------- .../behavior_path_planner_pull_out_design.md | 132 ++++++++ .../behavior_path_planner_pull_over_design.md | 234 ++++++++++++++ ...ver_freespace_parking_flowchart.drawio.svg | 287 ++++++++++++++++++ 4 files changed, 654 insertions(+), 161 deletions(-) create mode 100644 planning/behavior_path_planner/behavior_path_planner_pull_out_design.md create mode 100644 planning/behavior_path_planner/behavior_path_planner_pull_over_design.md create mode 100644 planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index c245cdf18d263..73d6eeb6ddf64 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -208,64 +208,6 @@ The Pull Over module is activated when goal is in the shoulder lane. Ego-vehicle - The ego-vehicle is stopped. - The speed is lower than threshold (default: < `0.01m/s`). -#### General parameters for pull_over - -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| request_length | [m] | double | when the ego-vehicle approaches the goal by this distance, the module is activated. | 200.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 2.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | - -#### **collision check** - -##### **occupancy grid based collision check** - -Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. - -##### Parameters for occupancy grid based collision check - -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid | - | bool | flag whether to use occupancy grid for collision check | true | -| use_occupancy_grid_for_longitudinal_margin | - | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | - | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | - | int | threshold of cell values to be considered as obstacles | 60 | - -##### Parameters for object recognition based collision check - -| Name | Unit | Type | Description | Default value | -| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | -| use_object_recognition | - | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 1.0 | - -#### **Goal Search** - -If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is -searched for in certain range of the shoulder lane. - -[Video of how goal search works](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) - -##### Parameters for goal search - -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| search_priority | - | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | -| enable_goal_research | - | double | flag whether to search goal | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 3.0 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 3.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | - #### **Path Generation** There are three path generation methods. @@ -282,65 +224,23 @@ The lateral jerk is searched for among the predetermined minimum and maximum val ![shift_parking](./image/shift_parking.drawio.svg) -[Video of how shift_parking works](https://user-images.githubusercontent.com/39142679/178034101-4dc61a33-bc49-41a0-a9a8-755cce53cbc6.mp4) - -###### Parameters for shift parking - -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | -| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | -| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | -| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | - ##### **geometric parallel parking** Generate two arc paths with discontinuous curvature. It stops twice in the middle of the path to control the steer on the spot. There are two path generation methods: forward and backward. See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. There is also [a simple python implementation](https://github.com/kosuke55/geometric-parallel-parking). -###### Parameters geometric parallel parking - -| Name | Unit | Type | Description | Default value | -| :---------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| arc_path_interval | [m] | double | interval between arc path points | 1.0 | -| pull_over_max_steer_rad | [rad] | double | maximum steer angle for path generation. it may not be possible to control steer up to max_steer_angle in vehicle_info when stopped | 0.35 | - ###### arc forward parking Generate two forward arc paths. ![arc_forward_parking](./image/arc_forward_parking.drawio.svg) -[Video of how arc_forward_parking works](https://user-images.githubusercontent.com/39142679/178034128-4754c401-8aff-4745-b69a-4a69ca29ce4b.mp4) - -###### Parameters arc forward parking - -| Name | Unit | Type | Description | Default value | -| :-------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------ | :------------ | -| enable_arc_forward_parking | [-] | bool | flag whether to enable arc forward parking | true | -| after_forward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | -| forward_parking_velocity | [m/s] | double | velocity when forward parking | 1.38 | -| forward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front left corner of ego-vehicle when forward parking | 0.0 | - ###### arc backward parking Generate two backward arc paths. ![arc_backward_parking](./image/arc_backward_parking.drawio.svg). -[Video of how arc_forward_parking works](https://user-images.githubusercontent.com/39142679/178034280-4b6754fe-3981-4aee-b5e0-970f34563c6d.mp4) - -###### Parameters arc backward parking - -| Name | Unit | Type | Description | Default value | -| :--------------------------------------- | :---- | :----- | :------------------------------------------------------------------------ | :------------ | -| enable_arc_backward_parking | [-] | bool | flag whether to enable arc backward parking | true | -| after_backward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | -| backward_parking_velocity | [m/s] | double | velocity when backward parking | -1.38 | -| backward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front right corner of ego-vehicle when backward | 0.0 | - #### Unimplemented parts / limitations for pull over - parking on the right shoulder is not allowed @@ -374,24 +274,6 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of - Exceeding the pull out end point by more than the threshold (default: `1.0m`) -#### General parameters for pull_out - -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | - -#### **Safe check with obstacles in shoulder lane** - -1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame) -2. Calculate object's polygon which is located in shoulder lane -3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path - -![pull_out_collision_check](./image/pull_out_collision_check.drawio.svg) - #### **Path Generation** There are two path generation methods. @@ -406,19 +288,6 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral ![shift_pull_out](./image/shift_pull_out.drawio.svg) -[Video of how shift pull out works](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4) - -###### parameters for shift pull out - -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | - ##### **geometric pull out** Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot. @@ -426,41 +295,12 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 ![geometric_pull_out](./image/geometric_pull_out.drawio.svg) -[Video of how geometric pull out works](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4) - -###### parameters for geometric pull out - -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :------------------------------------------------------ | :------------ | -| enable_geometric_pull_out | - | bool | flag whether to enable geometric pull out | true | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | - -#### **backward pull out start point search** - -If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`). - -![pull_out_after_back](./image/pull_out_after_back.drawio.svg) - -[Video of how pull out after backward driving works](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------------------------------------- | :------------- | :----- | :-------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_back | - | bool | flag whether to search backward for start_point | true | -| enable_back | - | bool | In the case of `efficient_path`, use efficient paths even if the back distance is longer. | -| In case of `short_back_distance`, use a path with as short a back distance | efficient_path | -| max_back_distance | [m] | double | maximum back distance | 30.0 | -| backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | -| backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | -| ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 | - #### Unimplemented parts / limitations for pull put - pull out from the right shoulder lane to the left lane is not allowed. - The safety of the road lane is not judged - Collision prediction is not performed for vehicles approaching from behind the road lane. +- freespace pull out is not yet implemented. ### Side Shift diff --git a/planning/behavior_path_planner/behavior_path_planner_pull_out_design.md b/planning/behavior_path_planner/behavior_path_planner_pull_out_design.md new file mode 100644 index 0000000000000..bd1bbbf70fea9 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_pull_out_design.md @@ -0,0 +1,132 @@ +# Pull Out design + +## Purpose / Role + +Pull out from the shoulder lane without colliding with objects. + +## Design + +```plantuml +@startuml +package pull_out{ + abstract class PullOutPlannerBase { + } + + + class ShiftPullOut { + } + + class GeometricPullOut { + } + + class PullOutModule { + } + + struct PullOutPath{} +} + +package utils{ + class PathShifter { + } + + class GeometricParallelParking { + } +} + +' pull out +ShiftPullOut --|> PullOutPlannerBase +GeometricPullOut --|> PullOutPlannerBase + +PathShifter --o ShiftPullOut +GeometricParallelParking --o GeometricPullOut + +PullOutPlannerBase --o PullOutModule + +PullOutPath --o PullOutPlannerBase + +@enduml +``` + +## General parameters for pull_out + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | + +## **Safe check with obstacles in shoulder lane** + +1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame) +2. Calculate object's polygon which is located in shoulder lane +3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path + +![pull_out_collision_check](./image/pull_out_collision_check.drawio.svg) + +### **Path Generation** + +There are two path generation methods. + +#### **shift pull out** + +Pull out distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one that generates a safe path is selected. + +- Generate the shoulder lane centerline and shift it to the current position. +- In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) +- Combine this path with center line of road lane + +![shift_pull_out](./image/shift_pull_out.drawio.svg) + +[shift pull out video](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4) + +##### parameters for shift pull out + +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | + +#### **geometric pull out** + +Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot. +See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. + +![geometric_pull_out](./image/geometric_pull_out.drawio.svg) + +[geometric pull out video](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4) + +##### parameters for geometric pull out + +| Name | Unit | Type | Description | Default value | +| :-------------------------- | :---- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths.　The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | + +### **backward pull out start point search** + +If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`). + +![pull_out_after_back](./image/pull_out_after_back.drawio.svg) + +[pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) + +#### **parameters for backward pull out start point search** + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | +| enable_back | [-] | bool | flag whether to search backward for start_point | true | +| search_priority | [-] | string | In the case of `efficient_path`, use efficient paths even if the back distance is longer. In case of `short_back_distance`, use a path with as short a back distance | efficient_path | +| max_back_distance | [m] | double | maximum back distance | 30.0 | +| backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | +| backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | +| ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 | diff --git a/planning/behavior_path_planner/behavior_path_planner_pull_over_design.md b/planning/behavior_path_planner/behavior_path_planner_pull_over_design.md new file mode 100644 index 0000000000000..2ec22a5050296 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_pull_over_design.md @@ -0,0 +1,234 @@ +# Pull Over design + +## Purpose / Role + +Search for a space where there are no objects and pull over there. + +## Design + +```plantuml +@startuml +package pull_over{ + abstract class PullOverPlannerBase { + } + abstract class GoalSeacherBase { + } + + package lane_parking <>{ + class ShiftPullOver { + } + class GeometricPullOver { + } + } + + package freespace_parking <>{ + class FreeSpacePullOver { + } + } + + class GoalSeacher { + } + + class PullOverModule { + } + + + struct GoalCandidates { + } + + struct PullOverPath{} +} + + +package utils{ + class PathShifter { + } + + class GeometricParallelParking { + } +} + +package freespace_planning_algorithms + { + class AstarSearch{} + class RRTStar{} +} + +' pull over +ShiftPullOver --|> PullOverPlannerBase +GeometricPullOver --|> PullOverPlannerBase +FreeSpacePullOver --|> PullOverPlannerBase +GoalSeacher --|> GoalSeacherBase + +PathShifter --o ShiftPullOver +GeometricParallelParking --o GeometricPullOver +AstarSearch --o FreeSpacePullOver +RRTStar --o FreeSpacePullOver + +PullOverPlannerBase --o PullOverModule +GoalSeacherBase --o PullOverModule + +PullOverPath --o PullOverPlannerBase +GoalCandidates --o GoalSeacherBase + +@enduml +``` + +## General parameters for pull_over + +| Name | Unit | Type | Description | Default value | +| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| request_length | [m] | double | when the ego-vehicle approaches the goal by this distance, the module is activated. | 200.0 | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 2.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | + +## **collision check** + +### **occupancy grid based collision check** + +Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. + +#### Parameters for occupancy grid based collision check + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid | [-] | bool | flag whether to use occupancy grid for collision check | true | +| use_occupancy_grid_for_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | + +### **onject recognition based collision check** + +#### Parameters for object recognition based collision check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 1.0 | + +## **Goal Search** + +If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is +searched for in certain range of the shoulder lane. + +[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) + +### Parameters for goal search + +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | +| search_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| enable_goal_research | [-] | double | flag whether to search goal | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 3.0 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 3.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | + +## **Path Generation** + +There are three path generation methods. +The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. + +### **shift parking** + +Pull over distance is calculated by the speed, lateral deviation, and the lateral jerk. +The lateral jerk is searched for among the predetermined minimum and maximum values, and the one satisfies ready conditions described above is output. + +1. Apply uniform offset to centerline of shoulder lane for ensuring margin +2. In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) +3. Combine this path with center line of road lane + +![shift_parking](./image/shift_parking.drawio.svg) + +[shift_parking video](https://user-images.githubusercontent.com/39142679/178034101-4dc61a33-bc49-41a0-a9a8-755cce53cbc6.mp4) + +#### Parameters for shift parking + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | + +### **geometric parallel parking** + +Generate two arc paths with discontinuous curvature. It stops twice in the middle of the path to control the steer on the spot. There are two path generation methods: forward and backward. +See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. There is also [a simple python implementation](https://github.com/kosuke55/geometric-parallel-parking). + +#### Parameters geometric parallel parking + +| Name | Unit | Type | Description | Default value | +| :---------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| arc_path_interval | [m] | double | interval between arc path points | 1.0 | +| pull_over_max_steer_rad | [rad] | double | maximum steer angle for path generation. it may not be possible to control steer up to max_steer_angle in vehicle_info when stopped | 0.35 | + +#### arc forward parking + +Generate two forward arc paths. + +![arc_forward_parking](./image/arc_forward_parking.drawio.svg) + +[how arc_forward_parking video](https://user-images.githubusercontent.com/39142679/178034128-4754c401-8aff-4745-b69a-4a69ca29ce4b.mp4) + +#### Parameters arc forward parking + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------ | :------------ | +| enable_arc_forward_parking | [-] | bool | flag whether to enable arc forward parking | true | +| after_forward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | +| forward_parking_velocity | [m/s] | double | velocity when forward parking | 1.38 | +| forward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front left corner of ego-vehicle when forward parking | 0.0 | + +#### arc backward parking + +Generate two backward arc paths. + +![arc_backward_parking](./image/arc_backward_parking.drawio.svg). + +[arc_forward_parking video](https://user-images.githubusercontent.com/39142679/178034280-4b6754fe-3981-4aee-b5e0-970f34563c6d.mp4) + +#### Parameters arc backward parking + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------- | :---- | :----- | :------------------------------------------------------------------------ | :------------ | +| enable_arc_backward_parking | [-] | bool | flag whether to enable arc backward parking | true | +| after_backward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | +| backward_parking_velocity | [m/s] | double | velocity when backward parking | -1.38 | +| backward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front right corner of ego-vehicle when backward | 0.0 | + +### freespace parking + +If the vehicle gets stuck with `lane_parking`, run `freespace_parking`. +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../costmap_generator/README.md) to `false` and `enable_freespace_parking` to `true` + +![pull_over_freespace_parking_flowchart](./image/pull_over_freespace_parking_flowchart.drawio.svg) +\*Series execution with `avoidance_module` in the flowchart is under development. + + + +#### Unimplemented parts / limitations for freespace parking + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. + +#### Parameters freespace parking + +| Name | Unit | Type | Description | Default value | +| :----------------------- | :--- | :--- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_freespace_parking | [-] | bool | This flag enables freespace parking, which runs when the vehicle is stuck due to e.g. obstacles in the parking area. | true | + +See [freespace_planner](../freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg b/planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg new file mode 100644 index 0000000000000..659fc2d979b84 --- /dev/null +++ b/planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg @@ -0,0 +1,287 @@ + + + + + + + + + + + + +
+
+
+ pull_over_module running +
+
+
+ + pull_over_module runn... + + + + + + + +
+
+
+ yes +
+
+
+ + yes + + + + + + + +
+
+
+ no +
+
+
+ + no + + + + + + +
+
+
+ need avoidance? +
+
+
+ + need avoidance? + + + + + + +
+
+
+ lane parking +
+
+
+ + lane parking + + + + + + + +
+
+
+ no +
+
+
+ + no + + + + + + + +
+
+
+ yes +
+
+
+ + yes + + + + + + +
+
+
+ can avoid with avoidance_module? +
+
+
+ + can avoid with... + + + + + + + + +
+
+
+ genete avoidance path +
+
+
+ + genete avoidance p... + + + + + + + + +
+
+
+ generate path with lane parking +
+
+
+ + generate path with la... + + + + + + +
+
+
+ stop(can't park) +
+
+
+ + stop(can't park) + + + + + + +
+
+
+ freespace parking +
+
+
+ + freespace parking + + + + + + +
+
+
+ can  generate path to the goal with freespace_parking? +
+
+
+ + can  generate path... + + + + + + + +
+
+
+ no +
+
+
+ + no + + + + + + + +
+
+
+ yes +
+
+
+ + yes + + + + + + + Text is not SVG - cannot display + + +