diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index 8150af7436212..40b64b7d87012 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -1,90 +1,173 @@ /**: ros__parameters: - # trajectory total/fixing length - trajectory_length: 300 # total trajectory length[m] - forward_fixing_distance: 5.0 # forward fixing length from base_link[m] - backward_fixing_distance: 3.0 # backward fixing length from base_link[m] - - # clearance(distance) when generating trajectory - clearance_from_road: 0.2 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road - - # clearance for unique points - 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 - range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending - clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line - - # avoiding param - 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] - center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, center line width around path points used for judging that object is required to avoid or not - acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range - - # solving quadratic programming - qp_max_iteration: 10000 # max iteration when solving QP - qp_eps_abs: 1.0e-8 # eps abs when solving OSQP - qp_eps_rel: 1.0e-11 # eps rel when solving OSQP - qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending - qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending - qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing - qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing - - # constrain space - is_getting_constraints_close2path_points: true # generate trajectory closer to path points - max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate - coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction - coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction - keep_space_shape_x: 0.2 # keep space for x direction from base_link[m] - keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, keep space for y direction from base_link[m] - max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m] - - is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid - enable_avoidance: true # enable avoidance function - is_using_vehicle_config: true # use vehicle config - num_sampling_points: 100 # number of optimizing points - num_joint_buffer_points: 3 # number of joint buffer points - num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - num_fix_points_for_extending: 50 # number of fixing points when extending - delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m] - 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 + 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 + + is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area + + # 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 + + 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] + + 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] + + 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 + + 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] + + 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] + + avoiding_object_type: + unknown: true + car: true + truck: true + bus: true + bicycle: true + motorbike: true + pedestrian: true + animal: true # mpt param - # vehicle param for mpt - max_steer_deg: 40.0 # max steering angle [deg] - steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] - - # trajectory param for mpt - 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] - forward_fixing_mpt_distance: 10 # number of fixing points around ego vehicle - - # optimization param for mpt - is_hard_fixing_terminal_point: false # hard fixing terminal point - base_point_weight: 2000.0 # slack weight for lateral error around base_link - top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point - mid_point_weight: 1000.0 # slack weight for lateral error around the middle point - lat_error_weight: 10.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - steer_input_weight: 0.01 # weight for steering input - steer_rate_weight: 1.0 # weight for steering rate - steer_acc_weight: 0.000001 # weight for steering acceleration - terminal_lat_error_weight: 0.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 - zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero + mpt: + option: + steer_limit_constraint: true + fix_points_around_ego: true + # plan_from_ego: false + visualize_sampling_num: 1 + enable_manual_warm_start: true + enable_warm_start: true # 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] + + kinematics: + max_steer_deg: 40.0 # max steering angle [deg] + + # 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 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 - min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m] - min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second] - max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m] - distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change + 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: + option: + # check if planned trajectory is outside drivable area + drivability_check: + # true: vehicle shape is considered as a set of circles + # false: vehicle shape is considered as footprint (= rectangle) + use_vehicle_circles: false + + # parameters only when use_vehicle_circles is true + vehicle_circles: + use_manual_vehicle_circles: false + num_for_constraints: 4 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.9 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 + + 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: + 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 + + 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: + use_manual_vehicle_circles: false + num_for_constraints: 3 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.8 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index 0c42f35d44206..bc694f79f46a1 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -19,12 +19,12 @@ find_package(OpenCV REQUIRED) ament_auto_add_library(obstacle_avoidance_planner SHARED src/vehicle_model/vehicle_model_interface.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp - src/util.cpp - src/debug.cpp + src/utils.cpp + src/costmap_generator.cpp + src/debug_visualization.cpp src/eb_path_optimizer.cpp src/mpt_optimizer.cpp - src/process_cv.cpp + src/cv_utils.cpp src/node.cpp ) diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/obstacle_avoidance_planner/README.md index f97e50aeea414..f2b3587274b06 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/obstacle_avoidance_planner/README.md @@ -1,9 +1,15 @@ -# Obstacle Avoidance Planner - ## 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, and velocity or acceleration will be updated in the latter modules. +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. + +## Feature + +This package is able to + +- follow the behavior path smoothly +- make the trajectory inside the drivable area as much as possible +- insert stop point if its trajectory point is outside the drivable area ## Inputs / Outputs @@ -13,34 +19,84 @@ Only position and orientation of trajectory are calculated in this module, and v | ---------------------------------------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | | `~/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_state` | nav_msgs/Odometry | Current Velocity of ego 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 | ### output -| Name | Type | Description | -| --------------- | -------------------------------------- | ----------------------------------------------------------------- | -| `~/output/path` | autoware_auto_planning_msgs/Trajectory | Optimized trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | -------------------------------------- | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs/Trajectory | Optimized trajectory that is feasible to drive and collision-free | ## Flowchart -Each module is explained briefly here based on the flowchart. +Flowchart of functions is explained here. + +```plantuml +@startuml +title pathCallback +start + +group generateOptimizedTrajectory + :checkReplan; + if (replanning required?) then (yes) + group getMaps + :getDrivableArea; + :getRoadClearanceMap; + :drawObstacleOnImage; + :getObstacleClearanceMap; + end group + + group optimizeTrajectory + :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 +end group + + +group generatePostProcessedTrajectory + :getExtendedOptimizedTrajectory; + :concatTrajectory; + :generateFineTrajectoryPoints; + :alignVelocity; +end group -![obstacle_avoidance_planner_flowchart](./media/obstacle_avoidance_planner_flowchart.drawio.svg) +:convertToTrajectory; -### Manage trajectory generation +:publishDebugDataInMain; -When one of the following conditions area realized, callback function to generate a trajectory is called and publish the trajectory. -Otherwise, previously generated trajectory is published. +stop +@enduml +``` -- Ego moves a certain distance compared to the previous ego pose (default: 10.0 [m]) +### checkReplan + +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. + +- Ego moves a certain distance compared to the previous ego pose (default: 3.0 [m]) - Time passes (default: 1.0 [s]) -- The path shape is changed (e.g. when starting planning lane change) - Ego is far from the previously generated trajectory -The previously generated trajectory is memorized, but it is not when the path shape is changed and ego is far from the previously generated trajectory. +### 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. -### Select objects to avoid +### 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. @@ -52,39 +108,37 @@ In detail, this equals to the following three conditions at the same time, and t ![obstacle_to_avoid](./media/obstacle_to_avoid.drawio.svg) -### Generate object costmap +### getObstacleClearanceMap Cost map is generated according to the distance to the target obstacles to be avoided. -### Generate road boundary costmap - -Cost map is generated according to the distance to the road boundaries. - -These cost maps area used in the optimization to generate a collision-free trajectory. +### getEBTrajectory -### Smooth path +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 assumes that the reference path is smooth enough. -Therefore the path from behavior is smoothed here, and send to the optimization as a format of trajectory. -Obstacles are not considered. +More details can be seen in the Elastic Band section. -### Optimize trajectory +### getModelPredictiveTrajectory This module makes the trajectory kinematically-feasible and collision-free. We define vehicle pose in the frenet coordinate, and minimize tracking errors by optimization. -This optimization also considers vehicle kinematics and collision checking with road boundary and obstacles. -To decrease the computation cost, the optimization is applied to the shorter trajectory than the whole trajectory, and concatenate the remained trajectory with the optimized one at last. +This optimization considers vehicle kinematics and collision checking with road boundary and obstacles. +To decrease the computation cost, the optimization is applied to the shorter trajectory (default: 50 [m]) than the whole trajectory, and concatenate the remained trajectory with the optimized one at last. The trajectory just in front of the ego must not be changed a lot so that the steering wheel will be stable. Therefore, we use the previously generated trajectory in front of the ego. Optimization center on the vehicle, that tries to locate just on the trajectory, can be tuned along side the vehicle vertical axis. -This parameter `optimization center offset` is defined as the signed length from the back-wheel center to the optimization center. +This parameter `mpt.kinematics.optimization center offset` is defined as the signed length from the back-wheel center to the optimization center. Some examples are shown in the following figure, and it is shown that the trajectory of vehicle shape differs according to the optimization center even if the reference trajectory (green one) is the same. ![mpt_optimization_offset](./media/mpt_optimization_offset.svg) -### Check drivability, and extend trajectory +More details can be seen in the Model Predictive Trajectory section. + +### insertZeroVelocityOutsideDrivableArea Optimized trajectory is too short for velocity planning, therefore extend the trajectory by concatenating the optimized trajectory and the behavior path considering drivability. Generated trajectory is checked if it is inside the drivable area or not, and if outside drivable area, output a trajectory inside drivable area with the behavior path or the previously generated trajectory. @@ -96,9 +150,9 @@ As described above, the behavior path is separated into two paths: one is for op - In this case, we do not care if the remained trajectory is inside or outside the drivable area since generally it is outside the drivable area (especially in a narrow road), but we want to pass a trajectory as long as possible to the latter module. - If optimized trajectory is **outside the drivable area**, and the remained trajectory is inside/outside the drivable area, - and if the previously generated trajectory **is memorized**, - - the output trajectory will be a part of previously generated trajectory, whose end firstly goes outside the drivable area. + - the output trajectory will be the previously generated trajectory, where zero velocity is inserted to the point firstly going outside the drivable area. - and if the previously generated trajectory **is not memorized**, - - the output trajectory will be a part of trajectory just transformed from the behavior path, whose end firstly goes outside the drivable area. + - the output trajectory will be a part of trajectory just transformed from the behavior path, where zero velocity is inserted to the point firstly going outside the drivable area. Optimization failure is dealt with the same as if the optimized trajectory is outside the drivable area. The output trajectory is memorized as a previously generated trajectory for the next cycle. @@ -107,14 +161,14 @@ _Rationale_ In the current design, since there are some modelling errors, the constraints are considered to be soft constraints. Therefore, we have to make sure that the optimized trajectory is inside the drivable area or not after optimization. -### Assign trajectory velocity +### alignVelocity -Velocity is assigned in the result trajectory by the behavior path. +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 smooth the path) and Model Predictive Trajectory (to optimize the trajectory) will be explained in detail. +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 @@ -131,11 +185,68 @@ Therefore the output path may have a collision with road boundaries or obstacles 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$), the objective function is as follows. +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. $$ -\min \sum_{k=1}^{n-1} |\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}| - |\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1}| +\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 @@ -152,7 +263,7 @@ When the optimization failed or the optimized trajectory is not collision free, 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. -#### Formulation +#### 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. @@ -170,6 +281,8 @@ y_{k+1} & = y_{k} + v \sin \theta_k 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$. @@ -200,47 +313,197 @@ $$ \end{align} $$ -Based on the linearization, the error kinematics is formulated with the following linear equations. +##### 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} \\ - \delta_{k+1} + \theta_{k+1} \end{pmatrix} = \begin{pmatrix} - 1 & v dt & 0 \\ - 0 & 1 & \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ - 0 & 0 & 1 - \frac{dt}{\tau} + 1 & v dt \\ + 0 & 1 \\ \end{pmatrix} \begin{pmatrix} y_k \\ \theta_k \\ - \delta_k \end{pmatrix} + \begin{pmatrix} 0 \\ - 0 \\ - \frac{dt}{\tau} + \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ \end{pmatrix} - \delta_{des} + \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\\ - 0 \end{pmatrix} \end{align} $$ -The objective function for smoothing and tracking is shown as follows. +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} -J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) \\ & = 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 + \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} $$ @@ -250,25 +513,29 @@ Assuming that the lateral distance to the road boundaries or obstacles from the $$ 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} +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 & (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) \\ & = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k}^2 + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k}^2 + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k}^2 +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{x}$, that concatenates all the design variables. +We define a vector $\boldsymbol{v}$, that concatenates all the design variables. $$ \begin{align} -\boldsymbol{x} = +\boldsymbol{v} = \begin{pmatrix} -... & y_k & \lambda_{\mathrm{base}, k} & \lambda_{\mathrm{top}, k} & \lambda_{\mathrm{mid}, k} & ... + \boldsymbol{u}'^T & \boldsymbol{\lambda}_\mathrm{base}^T & \boldsymbol{\lambda}_\mathrm{top}^T & \boldsymbol{\lambda}_\mathrm{mid}^T \end{pmatrix}^T \end{align} $$ @@ -277,7 +544,7 @@ The summation of these two objective functions is the objective function for the $$ \begin{align} -\min_{\boldsymbol{x}} J (\boldsymbol{x}) = \min_{\boldsymbol{x}} J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) + J_2 (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) +\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} $$ @@ -294,11 +561,215 @@ Finally we transform those objective functions to the following QP problem, and $$ \begin{align} -\min_{\boldsymbol{x}} \ & \frac{1}{2} \boldsymbol{x}^T \boldsymbol{P} \boldsymbol{x} + \boldsymbol{q} \boldsymbol{x} \\ -\mathrm{s.t.} \ & \boldsymbol{b}_l \leq \boldsymbol{A} \boldsymbol{x} \leq \boldsymbol{b}_u +\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` + +### 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. + +### Drivability in narrow roads + +- set `option.drivability_check.use_vehicle_circles` true + - use a set of circles as a shape of the vehicle when checking if the generated trajectory will be outside the drivable area. +- make `mpt.clearance.soft_clearance_from_road` smaller +- make `mpt.kinematics.optimization_center_offset` different + + - The point on the vehicle, offset forward from the base link` tries to follow the reference path. + + - This may cause the a part of generated trajectory will be outside the drivable area. + +### Computation time + +- Loose EB optimization + + - 1. make `eb.common.delta_arc_length_for_eb` large and `eb.common.num_sampling_points_for_eb` 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) + - 2. make `eb.qp.eps_abs` and `eb.qp.eps_rel` small + - This causes very unstable reference path generation for MPT, or turning a steering wheel a little bit larger + +- Enable computation reduction flag + + - 1. set l_inf_norm true (by default) + - use L-inf norm optimization for MPT w.r.t. slack variables, resulting in lower number of design variables + - 2. set enable_warm_start true + - 3. set enable_manual_warm_start true (by default) + - 4. set steer_limit_constraint false + - This causes no assumption for trajectory generation where steering angle will not exceeds its hardware limitation + - 5. make the number of collision-free constraints small + - How to change parameters depend on the type of collision-free constraints + - If + - This may cause the trajectory generation where a part of ego vehicle is out of drivable area + +- Disable publishing debug visualization markers + - set `option.is_publishing_*` false + +### Robustness + +- Check if the trajectory before EB, after EB, or after MPT is not robust + - if the trajectory before EB is not robust + - if the trajectory after EB is not robust + - if the trajectory after MPT is not robust + - make `mpt.weight.steer_input_weight` or `mpt.weight.steer_rate_weight` larger, which are stability of steering wheel along the trajectory. + +### Other options + +- `option.skip_optimization` skips EB and MPT optimization. +- `option.enable_pre_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. +- `mpt.option.plan_from_ego` enables planning from the ego pose when the ego's velocity is zero. +- `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. @@ -331,54 +802,51 @@ Although it has a cons to converge to the local minima, it can get a good soluti Topics for debugging will be explained in this section. -- **interpolated_points_marker** - - Trajectory points that is resampled from the input trajectory of obstacle avoidance planner. Whether this trajectory is inside the drivable area, smooth enough can be checked. - -![interpolated_points_marker](./media/interpolated_points_marker.png) - -- **smoothed_points_text** - - The output trajectory points from Elastic Band. Not points but small numbers are visualized. Whether these smoothed points are distorted or not can be checked. +- **Drivable area** + - 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` -![smoothed_points_text](./media/smoothed_points_text.png) +![drivable_area](./media/drivable_area.png) -- **(base/top/mid)\_bounds_line** - - Lateral Distance to the road boundaries to check collision in MPT (More precisely, - vehicle_width / 2.0). - - This collision check is done with three points on the vehicle (base = back wheel center, top, mid), therefore three line markers are visualized for each trajectory point. - - If the distance between the edge of line markers and the road boundaries is not about the half width of the vehicle, collision check will fail. +- **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. -![bounds_line](./media/bounds_line.png) +![behavior_path](./media/behavior_path.png) -- **optimized_points_marker** - - The output trajectory points from MPT. Whether the trajectory is outside the drivable area can be checked. +- **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. -![optimized_points_marker](./media/optimized_points_marker.png) +![eb_traj](./media/eb_traj.png) -- **Trajectory with footprint** - - Trajectory footprints can be visualized by TrajectoryFootprint of rviz_plugin. Whether trajectory footprints of input/output of obstacle_avoidance_planner is inside the drivable area or not can be checked. +- **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. -![trajectory_with_footprint](./media/trajectory_with_footprint.png) +![mpt_ref_traj](./media/mpt_ref_traj.png) -- **Drivable Area** - - Drivable area. When drivable area generation failed, the drawn area may be distorted. - - `nav_msgs/msg/OccupancyGrid` - - topic name: `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area` +- **MPT fixed trajectory** + - The fixed trajectory around the ego of model predictive trajectory. + - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![drivable_area](./media/drivable_area.png) +![mpt_fixed_traj](./media/mpt_fixed_traj.png) -- **area_with_objects** - - Area where obstacles are removed from the drivable area - - `nav_msgs/msg/OccupancyGrid` +- **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` -![area_with_objects](./media/area_with_objects.png) +![bounds](./media/bounds.png) -- **object_clearance_map** - - Cost map for obstacles (distance to the target obstacles to be avoided) - - `nav_msgs/msg/OccupancyGrid` +- **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. -![object_clearance_map](./media/object_clearance_map.png) +![mpt_traj](./media/mpt_traj.png) -- **clearance_map** - - Cost map for drivable area (distance to the road boundaries) - - `nav_msgs/msg/OccupancyGrid` +- **Output trajectory** + - The output of obstacle_avoidance_planner. Whether this trajectory is smooth enough can be checked. + - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![clearance_map](./media/clearance_map.png) +![output_traj](./media/output_traj.png) 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 11e1f8a905f12..40b64b7d87012 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -1,99 +1,173 @@ /**: ros__parameters: - # trajectory total/fixing length - trajectory_length: 200 # total trajectory length[m] - forward_fixing_distance: 20.0 # forward fixing length from base_link[m] - backward_fixing_distance: 5.0 # backward fixing length from base_link[m] - - # clearance(distance) when generating trajectory - clearance_from_road: 0.2 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road - - # clearance for unique points - 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 - range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending - clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line - - # avoiding param - 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] - center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. center line width around path points used for judging that object is required to avoid or not - acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range - avoiding_object_type: # avoiding object class - unknown: true - car: true - truck: true - bus: true - bicycle: true - motorbike: true - pedestrian: true - animal: true - - # solving quadratic programming - qp_max_iteration: 10000 # max iteration when solving QP - qp_eps_abs: 1.0e-8 # eps abs when solving OSQP - qp_eps_rel: 1.0e-11 # eps rel when solving OSQP - qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending - qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending - qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing - qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing - - # constrain space - is_getting_constraints_close2path_points: true # generate trajectory closer to path points - max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate - coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction - coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction - keep_space_shape_x: 0.2 # keep space for x direction from base_link[m] - keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. keep space for y direction from base_link[m] - max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m] - - is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid - enable_avoidance: true # enable avoidance function - is_using_vehicle_config: true # use vehicle config - num_sampling_points: 100 # number of optimizing points - num_joint_buffer_points: 3 # number of joint buffer points - num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - num_fix_points_for_extending: 50 # number of fixing points when extending - delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m] - 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 + 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 + + is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area + + # 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 + + 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] + + 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] + + 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 + + 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] + + 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] + + avoiding_object_type: + unknown: true + car: true + truck: true + bus: true + bicycle: true + motorbike: true + pedestrian: true + animal: true # mpt param - # vehicle param for mpt - max_steer_deg: 40.0 # max steering angle [deg] - steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] - - # trajectory param for mpt - 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] - forward_fixing_mpt_distance: 10 # forward fixing distance for MPT - - # optimization param for mpt - is_hard_fixing_terminal_point: false # hard fixing terminal point - base_point_weight: 2000.0 # slack weight for lateral error around base_link - top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point - mid_point_weight: 1000.0 # slack weight for lateral error around the middle point - lat_error_weight: 10.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - steer_input_weight: 0.01 # weight for steering input - steer_rate_weight: 1.0 # weight for steering rate - steer_acc_weight: 0.000001 # weight for steering acceleration - terminal_lat_error_weight: 0.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 - zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero + mpt: + option: + steer_limit_constraint: true + fix_points_around_ego: true + # plan_from_ego: false + visualize_sampling_num: 1 + enable_manual_warm_start: true + enable_warm_start: true # 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] + + kinematics: + max_steer_deg: 40.0 # max steering angle [deg] + + # 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 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 - min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m] - min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second] - max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m] - distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change + 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: + option: + # check if planned trajectory is outside drivable area + drivability_check: + # true: vehicle shape is considered as a set of circles + # false: vehicle shape is considered as footprint (= rectangle) + use_vehicle_circles: false + + # parameters only when use_vehicle_circles is true + vehicle_circles: + use_manual_vehicle_circles: false + num_for_constraints: 4 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.9 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 + + 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: + 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 + + 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: + use_manual_vehicle_circles: false + num_for_constraints: 3 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.8 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 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 new file mode 100644 index 0000000000000..5eba99ae6dfef --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -0,0 +1,255 @@ +// Copyright 2021 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__COMMON_STRUCTS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ + +#include "opencv2/core.hpp" +#include "opencv2/opencv.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 +#include +#include + +struct ReferencePoint; + +struct Bounds; +using VehicleBounds = std::vector; +using SequentialBounds = std::vector; + +using BoundsCandidates = std::vector; +using SequentialBoundsCandidates = std::vector; + +struct CVMaps +{ + cv::Mat drivable_area; + cv::Mat clearance_map; + cv::Mat only_objects_clearance_map; + cv::Mat area_with_objects_map; + nav_msgs::msg::MapMetaData map_info; +}; + +struct QPParam +{ + int max_iteration; + double eps_abs; + double eps_rel; +}; + +struct EBParam +{ + 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; +}; + +struct VehicleParam +{ + double wheelbase; + double length; + double width; + double rear_overhang; + double front_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; +}; + +struct DebugData +{ + struct StreamWithPrint + { + StreamWithPrint & operator<<(const std::string & s) + { + sstream << s; + if (s.back() == '\n') { + std::string tmp_str = sstream.str(); + debug_str += tmp_str; + + 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; + } + + StreamWithPrint & operator<<(const double d) + { + sstream << d; + return *this; + } + + std::string getString() const { return debug_str; } + + bool is_showing_calculation_time; + std::string debug_str = "\n"; + std::stringstream sstream; + }; + + 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 double local_vehicle_circle_radius, + const std::vector & local_vehicle_circle_longitudinal_offsets) + { + 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_radius = local_vehicle_circle_radius; + vehicle_circle_longitudinal_offsets = local_vehicle_circle_longitudinal_offsets; + } + + StreamWithPrint msg_stream; + size_t mpt_visualize_sampling_num; + geometry_msgs::msg::Pose current_ego_pose; + double vehicle_circle_radius; + 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 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; +}; + +struct Trajectories +{ + std::vector smoothed_trajectory; + std::vector mpt_ref_points; + std::vector model_predictive_trajectory; +}; + +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; +}; + +struct MPTParam +{ + bool enable_warm_start; + bool enable_manual_warm_start; + bool steer_limit_constraint; + bool fix_points_around_ego; + int num_curvature_sampling_points; + + std::vector vehicle_circle_longitudinal_offsets; // from base_link + double vehicle_circle_radius; + + 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; + + double terminal_lat_error_weight; + double terminal_yaw_error_weight; + double terminal_path_lat_error_weight; + double terminal_path_yaw_error_weight; + + double steer_input_weight; + double steer_rate_weight; + + double obstacle_avoid_lat_error_weight; + double obstacle_avoid_yaw_error_weight; + double obstacle_avoid_steer_input_weight; + + double optimization_center_offset; + double max_steer_rad; + + bool soft_constraint; + bool hard_constraint; + bool l_inf_norm; + bool two_step_soft_constraint; + bool plan_from_ego; +}; + +#endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp new file mode 100644 index 0000000000000..8165b523ca0d2 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp @@ -0,0 +1,66 @@ +// Copyright 2021 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__COSTMAP_GENERATOR_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include +#include + +class CostmapGenerator +{ +public: + CVMaps getMaps( + const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & objects, + const TrajectoryParam & traj_param, std::shared_ptr debug_data_ptr); + +private: + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + + cv::Mat getAreaWithObjects( + const cv::Mat & drivable_area, const cv::Mat & objects_image, + std::shared_ptr debug_data_ptr) const; + + cv::Mat getClearanceMap( + const cv::Mat & drivable_area, std::shared_ptr debug_data_ptr) const; + + cv::Mat drawObstaclesOnImage( + const bool enable_avoidance, + const std::vector & objects, + const std::vector & path_points, + const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, + const cv::Mat & clearance_map, const TrajectoryParam & traj_param, + std::vector * debug_avoiding_objects, + std::shared_ptr debug_data_ptr) const; + + cv::Mat getDrivableAreaInCV( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + std::shared_ptr debug_data_ptr) const; +}; +#endif // OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp new file mode 100644 index 0000000000000..b05a6edeb94d7 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp @@ -0,0 +1,121 @@ +// 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__CV_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" +#include "opencv2/core.hpp" +#include "opencv2/imgproc/imgproc_c.h" +#include "opencv2/opencv.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "geometry_msgs/msg/point32.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include + +namespace util +{ +struct Footprint; +} + +struct Edges +{ + int front_idx; + int back_idx; + geometry_msgs::msg::Point extended_front; + geometry_msgs::msg::Point extended_back; + geometry_msgs::msg::Point origin; +}; + +struct PolygonPoints +{ + std::vector points_in_image; + std::vector points_in_map; +}; + +namespace cv_utils +{ +void getOccupancyGridValue( + const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value); + +void putOccupancyGridValue( + nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value); +} // namespace cv_utils + +namespace cv_polygon_utils +{ +PolygonPoints getPolygonPoints( + const std::vector & points, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPoints( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPointsFromBB( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPointsFromCircle( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPointsFromPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +std::vector getCVPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const PolygonPoints & polygon_points, + const std::vector & path_points, + const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info); + +std::vector getDefaultCVPolygon( + const std::vector & points_in_image); + +std::vector getExtendedCVPolygon( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info); + +boost::optional getEdges( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info); +} // namespace cv_polygon_utils + +namespace cv_drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const VehicleParam & vehicle_param); + +bool isOutsideDrivableAreaFromCirclesFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const std::vector vehicle_circle_longitudinal_offsets, + const double vehicle_circle_radius); +} // namespace cv_drivable_area_utils + +#endif // OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp new file mode 100644 index 0000000000000..ead547892ee24 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp @@ -0,0 +1,47 @@ +// 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__DEBUG_VISUALIZATION_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_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_visualization +{ +visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( + const std::shared_ptr debug_data_ptr, + const std::vector & optimized_points, + const VehicleParam & vehicle_param, const bool is_showing_debug_detail); + +nav_msgs::msg::OccupancyGrid getDebugCostmap( + const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid); +} // namespace debug_visualization + +#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_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 index 7eb8d71adbacd..31c9943b4588b 100644 --- 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 @@ -11,266 +11,103 @@ // 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 -#include -#include +#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 -#include -#include -#include -#include -#include -#include +#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 +#include "boost/optional.hpp" #include #include -struct Bounds; -struct MPTParam; -struct ReferencePoint; - -struct Anchor -{ - geometry_msgs::msg::Pose pose; - double velocity; -}; - -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; -}; - -namespace autoware::common::osqp -{ -class OSQPInterface; -} - -namespace util -{ -struct Rectangle; -struct Footprint; -} // namespace util - -class MPTOptimizer; - -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; -}; - -struct ConstrainRectangles -{ - ConstrainRectangle object_constrain_rectangle; - ConstrainRectangle road_constrain_rectangle; -}; - -struct CandidatePoints -{ - std::vector fixed_points; - std::vector non_fixed_points; - int begin_path_idx; - int end_path_idx; -}; - -struct QPParam -{ - int max_iteration; - double eps_abs; - double eps_rel; - double eps_abs_for_extending; - double eps_rel_for_extending; - double eps_abs_for_visualizing; - double eps_rel_for_visualizing; -}; - -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; - int num_joint_buffer_points; - int num_joint_buffer_points_for_extending; - int num_offset_for_begin_idx; - int num_fix_points_for_extending; - double delta_arc_length_for_optimization; - double delta_arc_length_for_mpt_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_distance; - double forward_fixing_mpt_distance; - 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; - double max_dist_for_extending_end_point; -}; - -struct Trajectories -{ - std::vector smoothed_trajectory; - std::vector mpt_ref_points; - std::vector model_predictive_trajectory; - std::vector extended_trajectory; -}; - -struct ConstrainParam -{ - bool is_getting_constraints_close2path_points; - double clearance_for_fixing; - double clearance_for_straight_line; - double clearance_for_joint; - double range_for_extend_joint; - double clearance_for_only_smoothing; - double clearance_from_object_for_straight; - double min_object_clearance_for_joint; - double min_object_clearance_for_deceleration; - double clearance_from_road; - double clearance_from_object; - double extra_desired_clearance_from_road; - double max_x_constrain_search_range; - double coef_x_constrain_search_resolution; - double coef_y_constrain_search_resolution; - double keep_space_shape_x; - double keep_space_shape_y; - double max_lon_space_for_driveable_constraint; -}; - -struct VehicleParam -{ - double wheelbase; - double length; - double width; - double rear_overhang; - double front_overhang; - double max_steer_rad; - double steer_tau; -}; - -struct FOAData -{ - bool is_avoidance_possible = true; - std::vector avoiding_traj_points; - std::vector constrain_rectangles; -}; - -struct DebugData -{ - bool is_expected_to_over_drivable_area = false; - std::vector interpolated_points; - std::vector straight_points; - std::vector fixed_points; - std::vector non_fixed_points; - std::vector constrain_rectangles; - std::vector fixed_points_for_extending; - std::vector non_fixed_points_for_extending; - std::vector constrain_rectangles_for_extending; - std::vector interpolated_points_for_extending; - std::vector vehicle_footprints; - std::vector current_vehicle_footprints; - std::vector avoiding_traj_points; - std::vector smoothed_points; - std::vector avoiding_objects; - std::vector bounds; - std::vector bounds_candidate_for_base_points; - std::vector bounds_candidate_for_top_points; - std::vector bounds_candidate_for_mid_points; - std::vector fixed_mpt_points; - cv::Mat clearance_map; - cv::Mat only_object_clearance_map; - cv::Mat area_with_objects_map; - FOAData foa_data; -}; - -enum class OptMode : int { - Normal = 0, - Extending = 1, - Visualizing = 2, -}; - class EBPathOptimizer { private: - rclcpp::Clock logger_ros_clock_; + 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; + double velocity; + }; + + 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 ConstrainParam constrain_param_; + const EBParam eb_param_; const VehicleParam vehicle_param_; Eigen::MatrixXd default_a_matrix_; - std::unique_ptr keep_space_shape_ptr_; std::unique_ptr osqp_solver_ptr_; - std::unique_ptr ex_osqp_solver_ptr_; - std::unique_ptr vis_osqp_solver_ptr_; - std::unique_ptr mpt_optimizer_ptr_; + double current_ego_vel_; - void initializeSolver(); + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; Eigen::MatrixXd makePMatrix(); Eigen::MatrixXd makeAMatrix(); - geometry_msgs::msg::Pose getOriginPose( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points); - Anchor getAnchor( const std::vector & interpolated_points, const int interpolated_idx, const std::vector & path_points) const; - boost::optional>> getOccupancyPoints( - const geometry_msgs::msg::Pose & origin, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const; - Constrain getConstrainFromConstrainRectangle( const geometry_msgs::msg::Point & interpolated_point, const ConstrainRectangle & constrain_range); @@ -279,119 +116,21 @@ class EBPathOptimizer const double dx, const double dy, const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & opposite_point); - ConstrainRectangles getConstrainRectangles( - const Anchor & anchor, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info) const; - ConstrainRectangle getConstrainRectangle(const Anchor & anchor, const double clearance) const; ConstrainRectangle getConstrainRectangle( - const Anchor & anchor, const int & nearest_idx, - const std::vector & interpolated_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) const; - - ConstrainRectangle getConstrainRectangle( - const std::vector> & occupancy_map, - const std::vector> & occupancy_points, - const Anchor & anchor, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map) const; - - ConstrainRectangle getConstrainRectangle( - const std::vector & path_points, - const Anchor & anchor, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const; - - ConstrainRectangle getConstrainRectangle( - const std::vector> & occupancy_points, - const util::Rectangle & util_rect, const Anchor & anchor) const; - - ConstrainRectangle getUpdatedConstrainRectangle( - const ConstrainRectangle & rectangle, const geometry_msgs::msg::Point & candidate_point, - const nav_msgs::msg::MapMetaData & map_info, const cv::Mat & only_objects_clearance_map) const; - - OccupancyMaps getOccupancyMaps( - const std::vector> & occupancy_points, - const geometry_msgs::msg::Pose & origin_pose, - const geometry_msgs::msg::Point & origin_point_in_image, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info) const; + 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, const cv::Mat & only_objects_clearance, - const nav_msgs::msg::MapMetaData & map_info, + const int farthest_point_idx, std::vector & debug_detected_straight_points); - int getEndPathIdx( - const std::vector & path_points, - const int begin_path_idx, const double required_trajectory_length); - - int getEndPathIdxInsideArea( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const int begin_path_idx, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info); - - boost::optional> getPostProcessedConstrainRectangles( - const bool enable_avoidance, const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - const int farthest_point_idx, const int num_fixed_points, const int straight_idx, - DebugData * debug_data) const; - - boost::optional> getValidConstrainRectangles( - const std::vector & constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const; - - boost::optional> getConstrainRectanglesClose2PathPoints( - const bool is_using_only_smooth_constrain, const bool is_using_road_constrain, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const; - - boost::optional> getConstrainRectanglesWithinArea( - const bool is_using_only_smooth_constrain, const bool is_using_road_constrain, - const int farthest_point_idx, const int num_fixed_points, const int straight_idx, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - DebugData * debug_data) const; - - bool isPreFixIdx( - const int target_idx, const int farthest_point_idx, const int num_fixed, - const int straight_idx) const; - - bool isClose2Object( - const geometry_msgs::msg::Point & point, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map, const double distance_threshold) const; - - boost::optional> getConstrainRectangleVec( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data); - 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, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map); - - std::vector getConstrainRectangleVec( - const std::vector & input_path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx); - - Rectangle getRelShapeRectangle( - const geometry_msgs::msg::Vector3 & vehicle_shape, - const geometry_msgs::msg::Pose & origin) const; - - Rectangle getAbsShapeRectangle( - const Rectangle & rel_shape_rectangle_points, const geometry_msgs::msg::Point & offset_point, - const geometry_msgs::msg::Pose & origin) const; + const int farthest_point_idx, const int straight_idx); std::vector getPaddedInterpolatedPoints( const std::vector & interpolated_points, const int farthest_idx); @@ -402,18 +141,12 @@ class EBPathOptimizer boost::optional> getOptimizedTrajectory( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const CandidatePoints & candidate_points, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data); - - std::vector getExtendedOptimizedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points, - DebugData * debug_data); + const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, + std::shared_ptr debug_data_ptr); void updateConstrain( const std::vector & interpolated_points, - const std::vector & rectangle_points, const OptMode & opt_mode); + const std::vector & rectangle_points); std::vector convertOptimizedPointsToTrajectory( const std::vector optimized_points, const std::vector & constraint, @@ -422,50 +155,32 @@ class EBPathOptimizer std::vector getFixedPoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_optimized_points, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info); + 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, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info, DebugData * debug_data); - - bool isPointInsideDrivableArea( - const geometry_msgs::msg::Point & point, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info); + const std::unique_ptr & prev_trajs, std::shared_ptr debug_data_ptr); CandidatePoints getDefaultCandidatePoints( const std::vector & path_points); - std::vector solveQP(const OptMode & opt_mode); - - bool isFixingPathPoint( - const std::vector & path_points) const; + std::vector solveQP(); std::vector calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, - const OptMode & opt_mode); - - FOAData getFOAData( - const std::vector & rectangles, - const std::vector & interpolated_points, const int farthest_idx); + std::shared_ptr debug_data_ptr); public: EBPathOptimizer( - const bool is_showing_debug_info, const QPParam qp_param, const TrajectoryParam traj_param, - const ConstrainParam constrain_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param); - - ~EBPathOptimizer(); + const bool is_showing_debug_info, const TrajectoryParam & traj_param, const EBParam & eb_param, + const VehicleParam & vehicle_param); - boost::optional generateOptimizedTrajectory( - const bool enable_avoidance, const geometry_msgs::msg::Pose & ego_pose, - const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, - const std::vector & objects, - DebugData * debug_data); + 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, + std::shared_ptr debug_data_ptr); }; #endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_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 b8e760771d6f9..f7ff8c9f6c1e6 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 @@ -40,37 +40,77 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Sparse" +#include "interpolation/linear_interpolation.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "osqp_interface/osqp_interface.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include +#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 -#include -#include - -#include +#include "boost/optional.hpp" #include #include -namespace cv -{ -class Mat; -} +enum class CollisionType { NO_COLLISION = 0, OUT_OF_SIGHT = 1, OUT_OF_ROAD = 2, OBJECT = 3 }; -namespace autoware::common::osqp +struct Bounds { -class OSQPInterface; -} - -struct DebugData; -struct TrajectoryParam; -struct QPParam; -struct ConstrainParam; -struct VehicleParam; -struct Rectangle; -struct CVMaps; -struct Trajectories; + 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 = + interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); + 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}; + } + + void translate(const double offset) + { + lower_bound -= offset; + upper_bound -= offset; + } +}; struct ReferencePoint { @@ -78,220 +118,199 @@ struct ReferencePoint double k = 0; double v = 0; double yaw = 0; - geometry_msgs::msg::Quaternion q; double s = 0; - geometry_msgs::msg::Pose top_pose; - geometry_msgs::msg::Pose mid_pose; - double delta_yaw_from_p1 = 0; - double delta_yaw_from_p2 = 0; - boost::optional fix_state = boost::none; - Eigen::VectorXd optimized_state; + 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 = false; + 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 }; -struct Bounds +class MPTOptimizer { - struct SingleBounds +private: + struct MPTMatrix { - SingleBounds & operator=(std::vector & bounds) - { - ub = bounds[0]; - lb = bounds[1]; - return *this; - } - double ub; // left - double lb; // right - } c0, c1, c2; -}; - -struct MPTTrajs -{ - std::vector mpt; - std::vector ref_points; -}; + // 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; + }; + + struct ValueMatrix + { + Eigen::SparseMatrix Qex; + Eigen::SparseMatrix Rex; + }; -struct MPTMatrix -{ - Eigen::MatrixXd Aex; - Eigen::MatrixXd Bex; - Eigen::MatrixXd Wex; - Eigen::MatrixXd Cex; - Eigen::MatrixXd Qex; - Eigen::MatrixXd R1ex; - Eigen::MatrixXd R2ex; - Eigen::MatrixXd Uref_ex; -}; + struct ObjectiveMatrix + { + Eigen::MatrixXd hessian; + Eigen::VectorXd gradient; + }; -struct ObjectiveMatrix -{ - Eigen::MatrixXd hessian; - std::vector gradient; -}; + struct ConstraintMatrix + { + Eigen::MatrixXd linear; + Eigen::VectorXd lower_bound; + Eigen::VectorXd upper_bound; + }; -struct ConstraintMatrix -{ - Eigen::MatrixXd linear; - std::vector lower_bound; - std::vector upper_bound; -}; + struct MPTTrajs + { + std::vector mpt; + std::vector ref_points; + }; -struct MPTParam -{ - bool is_hard_fixing_terminal_point; - int num_curvature_sampling_points; - double base_point_dist_from_base_link; - double top_point_dist_from_base_link; - double mid_point_dist_from_base_link; - double clearance_from_road; - double clearance_from_object; - double base_point_weight; - double top_point_weight; - double mid_point_weight; - double lat_error_weight; - double yaw_error_weight; - double steer_input_weight; - double steer_rate_weight; - double steer_acc_weight; - double terminal_lat_error_weight; - double terminal_yaw_error_weight; - double terminal_path_lat_error_weight; - double terminal_path_yaw_error_weight; - double zero_ff_steer_angle; -}; + const double osqp_epsilon_ = 1.0e-3; -class MPTOptimizer -{ -private: bool is_showing_debug_info_; - - std::unique_ptr osqp_solver_ptr_; - std::unique_ptr mpt_param_ptr_; - std::unique_ptr traj_param_ptr_; - std::unique_ptr qp_param_ptr_; - std::unique_ptr constraint_param_ptr_; - std::unique_ptr vehicle_param_ptr_; + TrajectoryParam traj_param_; + VehicleParam vehicle_param_; + MPTParam mpt_param_; std::unique_ptr vehicle_model_ptr_; + std::unique_ptr osqp_solver_ptr_; - std::vector convertToReferencePoints( - const std::vector & points, - const std::vector & path_points, - const std::unique_ptr & prev_mpt_points, - const geometry_msgs::msg::Pose & ego_pose, const CVMaps & maps, DebugData * debug_data) const; + 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_; std::vector getReferencePoints( - const geometry_msgs::msg::Pose & origin_pose, const geometry_msgs::msg::Pose & ego_pose, const std::vector & points, - const std::vector & path_points, - const std::unique_ptr & prev_mpt_points, const CVMaps & maps, - DebugData * debug_data) const; + const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const; - std::vector getBaseReferencePoints( - const std::vector & interpolated_points, - const std::unique_ptr & prev_trajs, DebugData * debug_data) const; + void calcPlanningFromEgo(std::vector & ref_points) const; - void calcOrientation(std::vector * ref_points) const; + /* + std::vector convertToReferencePoints( + const std::vector & points, + const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const; + */ - void calcVelocity(std::vector * ref_points) const; + std::vector getFixedReferencePoints( + const std::vector & points, + const std::unique_ptr & prev_trajs) const; - void calcVelocity( - std::vector * ref_points, - const std::vector & points) const; + void calcBounds( + std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, + std::shared_ptr debug_data_ptr) const; - void calcCurvature(std::vector * ref_points) const; + void calcVehicleBounds( + std::vector & ref_points, const CVMaps & maps, + std::shared_ptr debug_data_ptr, const bool enable_avoidance) const; - void calcArcLength(std::vector * ref_points) const; + // void calcFixState( + // std::vector & ref_points, + // const std::unique_ptr & prev_trajs) const; - void calcExtraPoints(std::vector * ref_points) const; + void calcOrientation(std::vector & ref_points) const; - void calcFixPoints( - const std::unique_ptr & prev_trajs, const geometry_msgs::msg::Pose & ego_pose, - std::vector * ref_points, DebugData * debug_data) const; + void calcVelocity(std::vector & ref_points) const; - void calcInitialState( - std::vector * ref_points, const geometry_msgs::msg::Pose & origin_pose) const; + void calcVelocity( + std::vector & ref_points, + const std::vector & points) const; + + void calcCurvature(std::vector & ref_points) const; - void setOptimizedState(ReferencePoint * ref_point, const Eigen::Vector3d & optimized_state) const; + void calcArcLength(std::vector & ref_points) const; + + void calcExtraPoints( + 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,...]) */ - boost::optional generateMPTMatrix( + MPTMatrix generateMPTMatrix( const std::vector & reference_points, - const std::vector & path_points, - const std::unique_ptr & prev_trajs) const; + std::shared_ptr debug_data_ptr) const; - void addSteerWeightR(Eigen::MatrixXd * R, const std::vector & ref_points) const; + ValueMatrix generateValueMatrix( + const std::vector & reference_points, + const std::vector & path_points, + std::shared_ptr debug_data_ptr) const; - void addSteerWeightF(Eigen::VectorXd * f) const; + void addSteerWeightR( + std::vector> & Rex_triplet_vec, + const std::vector & ref_points) const; boost::optional executeOptimization( - const bool enable_avoidance, const MPTMatrix & m, - const std::vector & ref_points, - const std::vector & path_points, - const CVMaps & maps, DebugData * debug_data); + const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, + const std::vector & ref_points, std::shared_ptr debug_data_ptr); std::vector getMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpc_matrix, - const std::vector & optimized_points) const; - - double calcLateralError( - const geometry_msgs::msg::Point & target_point, const ReferencePoint & ref_point) const; - - Eigen::VectorXd getState( - const geometry_msgs::msg::Pose & ego_pose, const ReferencePoint & nearest_ref_point) const; - - std::vector getReferenceBounds( - const bool enable_avoidance, const std::vector & ref_points, - const CVMaps & maps, DebugData * debug_data) const; + std::vector & fixed_ref_points, + std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, + const MPTMatrix & mpt_matrix, std::shared_ptr debug_data_ptr); - boost::optional> getBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const; + std::vector getMPTFixedPoints( + const std::vector & ref_points) const; - boost::optional> getBoundCandidate( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps, - const double min_road_clearance, const double min_obj_clearance, const double rel_initial_lat, - const std::vector & rough_road_bound) const; + BoundsCandidates getBoundsCandidates( + const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const; - boost::optional> getRoughBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const; + CollisionType getCollisionType( + const CVMaps & maps, const bool enable_avoidance, + const geometry_msgs::msg::Pose & avoiding_point, const double traversed_dist, + const double bound_angle) const; - double getTraversedDistance( - const bool enable_avoidance, const ReferencePoint & ref_point, const double traverse_angle, - const double initial_value, const CVMaps & maps, const double min_road_clearance, - const double min_obj_clearance, const bool search_expanding_side = false) const; - - boost::optional getValidLatError( - const bool enable_avoidance, const ReferencePoint & ref_point, const double initial_value, - const CVMaps & maps, const double min_road_clearance, const double min_obj_clearance, - const std::vector & rough_road_bound, const bool search_expanding_side = false) const; - - // TODO(unknown): refactor replace all relevant funcs boost::optional getClearance( const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, const nav_msgs::msg::MapMetaData & map_info) const; - ObjectiveMatrix getObjectiveMatrix(const Eigen::VectorXd & x0, const MPTMatrix & m) const; + ObjectiveMatrix getObjectiveMatrix( + const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, + [[maybe_unused]] const std::vector & ref_points, + std::shared_ptr debug_data_ptr) const; ConstraintMatrix getConstraintMatrix( - const bool enable_avoidance, const Eigen::VectorXd & x0, const MPTMatrix & m, - const CVMaps & maps, const std::vector & ref_points, - const std::vector & path_points, - DebugData * debug_data) const; + const bool enable_avoidance, const MPTMatrix & mpt_mat, + const std::vector & ref_points, + std::shared_ptr debug_data_ptr) const; public: MPTOptimizer( - const bool is_showing_debug_info, const QPParam & qp_param, const TrajectoryParam & traj_param, - const ConstrainParam & constrain_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param); + const bool is_showing_debug_info, const TrajectoryParam & traj_param, + const VehicleParam & vehicle_param, const MPTParam & mpt_param); boost::optional getModelPredictiveTrajectory( const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, const std::unique_ptr & prev_trajs, const CVMaps & maps, - const geometry_msgs::msg::Pose & ego_pose, DebugData * debug_data); + const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, + std::shared_ptr debug_data_ptr); }; #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 e184847cd7e7f..56f4e437853f3 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -14,207 +14,228 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/costmap_generator.hpp" +#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "opencv2/core.hpp" +#include "rclcpp/clock.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 -#include #include -namespace ros +namespace { -class Time; -} - -namespace cv +template +double lerpTwistX( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { -class Mat; + constexpr double epsilon = 1e-6; + + const double closest_to_target_dist = + tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); + const double seg_dist = + tier4_autoware_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; + + return std::abs(seg_dist) < epsilon + ? next_vel + : interpolation::lerp(closest_vel, next_vel, closest_to_target_dist / seg_dist); } -namespace tf2_ros +template +double lerpPoseZ( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { -class Buffer; -class TransformListener; -} // namespace tf2_ros + constexpr double epsilon = 1e-6; -class EBPathOptimizer; + const double closest_to_target_dist = + tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); + const double seg_dist = + tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); -struct QPParam; -struct TrajectoryParam; -struct ConstrainParam; -struct VehicleParam; -struct MPTParam; -struct DebugData; -struct Trajectories; + 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 { private: OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rclcpp::Clock logger_ros_clock_; + int eb_solved_count_; + 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_using_vehicle_config_; + bool is_showing_calculation_time_; bool is_stopping_if_outside_drivable_area_; bool enable_avoidance_; - const int min_num_points_for_getting_yaw_; - std::mutex mutex_; - - // params outside logic - double min_delta_dist_for_replan_; - double min_delta_time_sec_for_replan_; - double max_dist_for_extending_end_point_; - double distance_for_path_shape_change_detection_; + bool enable_pre_smoothing_; + bool skip_optimization_; + bool reset_prev_optimization_; + + // vehicle circles info for drivability check + bool use_vehicle_circles_for_drivability_; + bool use_manual_vehicle_circles_for_drivability_; + int vehicle_circle_constraints_num_for_drivability_; + int vehicle_circle_radius_num_for_drivability_; + double vehicle_circle_radius_ratio_for_drivability_; + double vehicle_circle_radius_for_drivability_; + std::vector vehicle_circle_longitudinal_offsets_for_drivability_; + + // vehicle circles info for for mpt constraints + bool use_manual_vehicle_circles_for_mpt_; + int vehicle_circle_constraints_num_for_mpt_; + int vehicle_circle_radius_num_for_mpt_; + double vehicle_circle_radius_ratio_for_mpt_; + + // 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_; // logic + std::unique_ptr costmap_generator_ptr_; std::unique_ptr eb_path_optimizer_ptr_; + std::unique_ptr mpt_optimizer_ptr_; // params - std::unique_ptr qp_param_; - std::unique_ptr traj_param_; - std::unique_ptr constrain_param_; - std::unique_ptr vehicle_param_; - std::unique_ptr mpt_param_; - - std::unique_ptr current_ego_pose_ptr_; + TrajectoryParam traj_param_; + EBParam eb_param_; + VehicleParam vehicle_param_; + MPTParam mpt_param_; + int mpt_visualize_sampling_num_; + + // debug + mutable std::shared_ptr debug_data_ptr_; + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + + geometry_msgs::msg::Pose current_ego_pose_; std::unique_ptr current_twist_ptr_; std::unique_ptr prev_ego_pose_ptr_; - std::unique_ptr prev_trajectories_ptr_; + std::unique_ptr prev_optimal_trajs_ptr_; std::unique_ptr> prev_path_points_ptr_; - std::unique_ptr in_objects_ptr_; + std::unique_ptr objects_ptr_; - // TF - std::unique_ptr tf_buffer_ptr_; - std::unique_ptr tf_listener_ptr_; - std::unique_ptr prev_replanned_time_ptr_; + std::unique_ptr latest_replanned_time_ptr_; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; // ROS - rclcpp::Publisher::SharedPtr trajectory_pub_; - rclcpp::Publisher::SharedPtr avoiding_traj_pub_; + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr + debug_extended_fixed_traj_pub_; rclcpp::Publisher::SharedPtr - debug_smoothed_points_pub_; - rclcpp::Publisher::SharedPtr - is_avoidance_possible_pub_; + 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_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::Subscription::SharedPtr path_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr is_avoidance_sub_; - // callback functions - void pathCallback(const autoware_auto_planning_msgs::msg::Path::SharedPtr); + // param callback function + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + // subscriber callback functions void odomCallback(const nav_msgs::msg::Odometry::SharedPtr); void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr); void enableAvoidanceCallback(const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr); + void pathCallback(const autoware_auto_planning_msgs::msg::Path::SharedPtr); - void initialize(); - - // generate fine trajectory - std::vector generatePostProcessedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::vector & merged_optimized_points) - const; - - bool needReplan( - const geometry_msgs::msg::Pose & ego_pose, - const std::unique_ptr & prev_ego_pose, - const std::vector & path_points, - const std::unique_ptr & previous_replanned_time, - const std::unique_ptr> & - prev_path_points, - std::unique_ptr & prev_traj_points); + // functions + void resetPlanning(); + void resetPrevOptimization(); std::vector generateOptimizedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & input_path); - std::unique_ptr getCurrentEgoPose(); - - bool isPathShapeChanged( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr> & - prev_path_points); + bool checkReplan(const std::vector & path_points); - autoware_auto_planning_msgs::msg::Trajectory generateTrajectory( - const autoware_auto_planning_msgs::msg::Path & in_path); + Trajectories optimizeTrajectory( + const autoware_auto_planning_msgs::msg::Path & path, const CVMaps & cv_maps); - std::vector convertPointsToTrajectory( - const std::vector & path_points, - const std::vector & trajectory_points) const; + Trajectories getPrevTrajs( + const std::vector & path_points) const; - void publishingDebugData( - const DebugData & debug_data, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & traj_points, - const VehicleParam & vehicle_param); + void insertZeroVelocityOutsideDrivableArea( + std::vector & traj_points, + const CVMaps & cv_maps); - int calculateNonDecelerationRange( - const std::vector & traj_points, - const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Twist & ego_twist) const; + void publishDebugDataInOptimization( + const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & traj_points); - Trajectories getTrajectoryInsideArea( - const Trajectories & trajs, + Trajectories makePrevTrajectories( const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data) const; + const Trajectories & trajs); - boost::optional calcTrajectoryInsideArea( - const Trajectories & trajs, + std::vector generatePostProcessedTrajectory( const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data, const bool is_prev_traj = false) const; - - Trajectories getPrevTrajs( - const std::vector & path_points) const; - - std::vector getPrevTrajectory( - const std::vector & path_points) const; + const std::vector & merged_optimized_points); - Trajectories makePrevTrajectories( - const geometry_msgs::msg::Pose & ego_pose, + std::vector getExtendedTrajectory( const std::vector & path_points, - const Trajectories & trajs) const; + const std::vector & optimized_points); - Trajectories getBaseTrajectory( + std::vector generateFineTrajectoryPoints( const std::vector & path_points, - const Trajectories & current_trajs) const; + const std::vector & traj_points) const; - boost::optional getStopIdx( + std::vector alignVelocity( + const std::vector & fine_traj_points, const std::vector & path_points, - const Trajectories & trajs, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & road_clearance_map, DebugData * debug_data) const; - - void declareObstacleAvoidancePlannerParameters(); + const std::vector & traj_points) const; - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); + void publishDebugDataInMain(const autoware_auto_planning_msgs::msg::Path & path) const; public: explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); - ~ObstacleAvoidancePlanner(); }; #endif // OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp deleted file mode 100644 index 1ad7a77cf416a..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp +++ /dev/null @@ -1,203 +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__UTIL_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTIL_HPP_ - -#include -#include - -#include -#include - -#include - -#include -#include - -struct VehicleParam; -struct ReferencePoint; -struct Trajectories; -struct TrajectoryParam; - -namespace util -{ -template -geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const T & point, const geometry_msgs::msg::Pose & origin); - -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin); - -double calculate2DDistance( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b); - -double calculateSquaredDistance( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b); - -double getYawFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); - -double normalizeRadian(const double angle); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); - -geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double yaw); - -template -geometry_msgs::msg::Point transformMapToImage( - const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info); - -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); - -bool interpolate2DPoints( - const std::vector & x, const std::vector & y, const double resolution, - std::vector & interpolated_points); - -std::vector getInterpolatedPoints( - const std::vector & first_points, - const std::vector & second_points, const double delta_arc_length); - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length); - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length); - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length); - -template -std::vector getInterpolatedPoints( - const T & points, const double delta_arc_length); - -template -int getNearestIdx( - const T & points, const geometry_msgs::msg::Pose & pose, const int default_idx, - const double delta_yaw_threshold); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const int default_idx, const double delta_yaw_threshold); - -int getNearestIdxOverPoint( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const int default_idx, const double delta_yaw_threshold); - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point, const int default_idx); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Point & point); - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point); - -int getNearestIdx( - const std::vector & points, const double target_s, const int begin_idx); - -template -int getNearestPointIdx(const T & points, const geometry_msgs::msg::Point & point); - -std::vector convertPathToTrajectory( - const std::vector & path_points); - -std::vector -convertPointsToTrajectoryPointsWithYaw(const std::vector & points); - -std::vector fillTrajectoryWithVelocity( - const std::vector & traj_points, - const double velocity); - -template -std::vector alignVelocityWithPoints( - const std::vector & traj_points, - const T & points, const int zero_velocity_traj_idx, const int max_skip_comparison_idx); - -struct Rectangle -{ - int min_x_idx = 0; - int min_y_idx = 0; - int max_x_idx = 0; - int max_y_idx = 0; - int area = 0; -}; - -std::vector> getHistogramTable(const std::vector> & input); - -Rectangle getLargestRectangleInRow( - const std::vector & histo, const int current_row, const int row_size); - -Rectangle getLargestRectangle(const std::vector> & input); - -boost::optional getLastExtendedPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold); - -boost::optional getLastExtendedTrajPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold); - -struct Footprint -{ - geometry_msgs::msg::Point p; - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; -}; - -std::vector getVehicleFootprints( - const std::vector & optimized_points, - const VehicleParam & vehicle_param); - -std::vector calcEuclidDist(const std::vector & x, const std::vector & y); - -bool hasValidNearestPointFromEgo( - const geometry_msgs::msg::Pose & ego_pose, const Trajectories & trajs, - const TrajectoryParam & traj_param); - -std::vector concatTraj( - const Trajectories & trajs); - -int getZeroVelocityIdx( - const bool is_showing_debug_info, const std::vector & fine_points, - const std::vector & path_points, - const std::unique_ptr & opt_trajs, const TrajectoryParam & traj_param); - -template -int getZeroVelocityIdxFromPoints( - const T & points, const std::vector & fine_points, - const int /* default_idx */, const TrajectoryParam & traj_param); - -template -double getArcLength(const T & points, const int initial_idx = 0); - -double getArcLength( - const std::vector & points, const int initial_idx = 0); - -void logOSQPSolutionStatus(const int solution_status); - -} // namespace util - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTIL_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp new file mode 100644 index 0000000000000..602c5db22e0ac --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -0,0 +1,336 @@ +// 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_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.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 + +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); + +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::Point pos, const double backward_length, + const double delta_length) +{ + if (points.empty()) { + return T{}; + } + + const size_t target_idx = tier4_autoware_utils::findNearestIndex(points, pos); + + 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 +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); +} // namespace utils + +#endif // OBSTACLE_AVOIDANCE_PLANNER__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 a7e623bc099c3..6cce41c70f227 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 @@ -48,10 +48,11 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" -#include -#include +#include /** * @class vehicle model class of bicycle kinematics @@ -64,9 +65,8 @@ class KinematicsBicycleModel : public VehicleModelInterface * @brief constructor with parameter initialization * @param [in] wheelbase wheelbase length [m] * @param [in] steer_lim steering angle limit [rad] - * @param [in] steer_tau steering time constant for 1d-model */ - KinematicsBicycleModel(const double wheelbase, const double steer_lim, const double steer_tau); + KinematicsBicycleModel(const double wheel_base, const double steer_limit); /** * @brief destructor @@ -74,26 +74,31 @@ class KinematicsBicycleModel : public VehicleModelInterface virtual ~KinematicsBicycleModel() = default; /** - * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk + * @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] Cd coefficient matrix * @param [out] Wd coefficient matrix - * @param [in] dt Discretization arc length + * @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 calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) override; + 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; - -private: - double wheelbase_; //!< @brief wheelbase length [m] - double steer_lim_; //!< @brief steering angle limit [rad] - double steer_tau_; //!< @brief steering time constant for 1d-model }; #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_interface.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp index 52f99e46aacc6..55de7dadc3d03 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 @@ -22,7 +22,10 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Sparse" + +#include /** * @class vehicle model class @@ -31,20 +34,23 @@ class VehicleModelInterface { protected: - const int dim_x_; //!< @brief dimension of state 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 + 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] public: /** * @brief constructor - * @param [in] dim_x dimension of state x + * @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); + VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheel_base, double steer_limit); /** * @brief destructor @@ -52,8 +58,8 @@ class VehicleModelInterface virtual ~VehicleModelInterface() = default; /** - * @brief get state x dimension - * @return state dimension + * @brief get kinematics x dimension + * @return kinematics dimension */ int getDimX(); @@ -69,6 +75,8 @@ class VehicleModelInterface */ int getDimY(); + void updateCenterOffset(const double center_offset_from_base); + /** * @brief set curvature * @param [in] curvature curvature on the linearized point on path @@ -76,16 +84,26 @@ class VehicleModelInterface void setCurvature(const double curvature); /** - * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk + * @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] Cd coefficient matrix * @param [out] Wd coefficient matrix - * @param [in] ds Discretization arc length + * @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 calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) = 0; + virtual void calculateObservationSparseMatrix(std::vector> & Cd_vec) = 0; /** * @brief calculate reference input 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 e0ddc12072bd3..478c692989aea 100644 --- a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml +++ b/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml @@ -3,7 +3,6 @@ - @@ -13,6 +12,5 @@ - diff --git a/planning/obstacle_avoidance_planner/media/behavior_path.png b/planning/obstacle_avoidance_planner/media/behavior_path.png new file mode 100644 index 0000000000000..cc99ffe44e1ec Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/behavior_path.png differ diff --git a/planning/obstacle_avoidance_planner/media/bounds.png b/planning/obstacle_avoidance_planner/media/bounds.png new file mode 100644 index 0000000000000..54627b34f10ca Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/bounds.png differ diff --git a/planning/obstacle_avoidance_planner/media/drivable_area.png b/planning/obstacle_avoidance_planner/media/drivable_area.png index a1c855323e579..6606299705143 100644 Binary files a/planning/obstacle_avoidance_planner/media/drivable_area.png and b/planning/obstacle_avoidance_planner/media/drivable_area.png differ diff --git a/planning/obstacle_avoidance_planner/media/eb_traj.png b/planning/obstacle_avoidance_planner/media/eb_traj.png new file mode 100644 index 0000000000000..9910810920bff Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/eb_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png b/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png new file mode 100644 index 0000000000000..2344467811a83 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png b/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png new file mode 100644 index 0000000000000..6129ac23dc008 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_traj.png b/planning/obstacle_avoidance_planner/media/mpt_traj.png new file mode 100644 index 0000000000000..44a06bc1ef9f6 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/mpt_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/output_traj.png b/planning/obstacle_avoidance_planner/media/output_traj.png new file mode 100644 index 0000000000000..e4cfae92d0a8f Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/output_traj.png differ diff --git a/planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md b/planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md new file mode 100644 index 0000000000000..8610070c4871d --- /dev/null +++ b/planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md @@ -0,0 +1,330 @@ +# Obstacle Avoidance Planner + +## Purpose + +obstacle_avoidance_planner は入力された path と drivable area、および動物体情報をもとに、車両キネマティクスモデルを考慮して車が走行可能な軌道を生成する。trajectory 内の各経路点の位置姿勢のみ計画しており、速度や加速度の計算は後段の別モジュールで行われる。 + + + +## Inputs / Outputs + +### input + +- reference_path [`autoware_auto_planning_msgs/Path`] : Reference path and the corresponding drivable area. +- objects [`autoware_auto_perception_msgs/PredictedObjects`] : Recognized objects around the vehicle + +### output + +- optimized_trajectory [`autoware_auto_planning_msgs/Trajectory`] : Optimized trajectory that is feasible to drive and collision-free. + +## Flowchart + +フローチャートとともに、各機能の概要をおおまかに説明する。 + +![obstacle_avoidance_planner_flowchart](./media/obstacle_avoidance_planner_flowchart.drawio.svg) + +### Manage trajectory generation + +以下の条件のいずれかが満たされたときに、経路生成の関数を呼ぶ。それ以外の時は前回生成された経路を出力する。 + +- 前回経路生成時から一定距離走行 (default: 10.0 [m]) +- 一定時間経過 (default: 1.0 [s]) +- レーンチェンジなどで入力の path の形状が変わった時 +- 自車位置が前回経路から大きく外れた時 + +入力の path の形状が変わった時と自車が前回経路から大きく外れた時は、保持している前回経路を破棄するリセット機能もある。 + +### Select objects to avoid + +静的で路肩にある障害物のみ回避対象とする。 +具体的には、以下の 3 つの条件を満たすものであり、図で示すと赤い id: 3, 4, 5 の物体である。 + +- 指定された速度以下 (default: 0.1 [m/s]) +- 物体重心が center line 上に存在しない（前車追従の車を避けないようにするため） +- 少なくとも 1 つ以上の物体形状ポリゴン点が drivable area に存在する。 + +![obstacle_to_avoid](./media/obstacle_to_avoid.drawio.svg) + +### Generate object costmap + +回避対象である障害物からの距離に応じたコストマップを生成する。 + +### Generate road boundary costmap + +道路からの距離に応じたコストマップを生成する。 + +これらのコストマップは後段の Optimize trajectory の手法である Model predictive trajectory の障害物・道路境界に衝突しない制約条件を定式化する際に使用される。 + +### Smooth path + +後段の最適化処理で曲率のなめらかな参照経路が必要であるため、最適化前に path をなめらかにして trajectory の形式で出力する。 +この平滑化は障害物を考慮しない。 + +### Optimize trajectory + +参照経路に基づいたフレネ座標系で車両キネマティクス、及び追従誤差を定義し、二次計画法を用いて車両キネマティクスや障害物回避を考慮しながら追従誤差が小さくなるように経路生成する。 +自車近傍の経路の急な変化を防ぐため、直近の経路は前回の経路をそのまま使用する。 +計算コストを抑えるため、最終的に出力する経路長よりも短い距離に対してのみ計算を行う。 + +### Extend trajectory + +経路を規定の長さ（default: 200m）に拡張するため、最適化した経路の先を Reference 経路と接続する。 + +### Check drivable area + +生成された経路が走行可能領域を逸脱していた場合、前回生成された走行可能領域内にある経路を出力する。 + +_Rationale_ +現在の設計において、数値誤差による破綻を防ぐために障害物回避は全てソフト制約として考慮されており、生成された経路に置いて車両が走行可能領域内に入っているかの判定が必要。 + +### Apply path velocity + +入力の path に埋め込まれている速度を出力される trajectory に埋め込む。経路間隔が異なるため線形補間を用いる。 + +## Algorithms + +Smooth path で使われている Elastic band と、Optimized trajectory で使われている Model predictive trajectory の詳細な説明をする。 + +### Elastic band + +#### 概要 + +behavior_path_planner で計算された path は場合によってはなめらかではない可能性があるので、その path の平滑化をすることが目的である。 + +次の Model predictive trajectory でも平滑化項は入っているが、目標経路になるべく追従しようとする項も入っているため、目標経路がなめらかでなかった場合はこの 2 つの項が反発する。 +それを防ぐためにここで平滑化のみを行っている。 +また Model predictive trajectory では各点における曲率と法線を元に最適化しており、平滑化された目標経路を渡すことで最適化の結果を安定させる狙いもある。 + +平滑化の際、障害物や道路壁を考慮していないため障害物や道路壁に衝突した trajectory が計算されることもある。 + +この Elastic band と次の Model predictive trajectory は、計算負荷を抑えるためにある程度の長さでクリップした trajectory を出力するようになっている。 + +#### 数式 + +前後の点からの距離の差の二乗を目的関数とする二次計画。 + +各点は一定の範囲以内しか動かないという制約を設けることで、入力の軌道をよりなめらかにした軌道を得る。 + +$\boldsymbol{p}_k$を$k$番目の経路点の座標ととしたとき以下のコスト関数を最小化する二次計画を解く。ここでは始点と終点である$\boldsymbol{p}_0$と$\boldsymbol{p}_n$は固定である。 + +$$ +\min \sum_{k=1}^{n-1} |\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}| - |\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1}| +$$ + +### Model predictive trajectory + +#### 概要 + +Elastic band で平滑化された trajectory に対して、以下の条件を満たすように修正を行うことが目的である。 + +- 線形化された車両のキネマティクスモデルに基づき走行可能である +- 障害物や道路壁面に衝突しない + +障害物や道路壁面に衝突しない条件はハードではなくソフト制約として含まれている。車両の後輪位置、前輪位置、その中心位置において障害物・道路境界との距離から制約条件が計算されている。 +条件を満たさない解が出力された場合は、後段の後処理で弾かれ、前の周期で計画された trajectory を出力する。 + +自車付近の経路が振動しないように、自車近傍の経路点を前回の経路点と一致させる制約条件も含まれており、これが唯一の二次計画法のハード制約である。 + +#### 数式 + +以下のように、経路に対して車両が追従するときの bicycle kinematics model を考える。 +時刻$k$における、経路上の車両の最近傍点の座標($x$座標は経路の接線に平行)から見た追従誤差に関して、横偏差$y_k$、向きの偏差$\theta_k$、ステア角$\delta_k$と定める。 + +![vehicle_error_kinematics](./media/vehicle_error_kinematics.png) + +指令ステア角度を$\delta_{des, k}$とすると、ステア角の遅延を考慮した車両キネマティクスモデルは以下で表される。この時、ステア角$\delta_k$は一次遅れ系として指令ステア角に追従すると仮定する。 + +$$ +\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} +$$ + +次にこれらの式を線形化する。$y_k$, $\theta_k$は追従誤差であるため微小近似でき、$\sin \theta_k \approx \theta_k$となる。 + +$\delta_k$に関してはステア角であるため微小とみなせない。そこで、以下のように参照経路の曲率$\kappa_k$から計算されるステア角$\delta_{\mathrm{ref}, k}$を用いることにより、$\delta_k$を微小な値$\Delta \delta_k$で表す。 + +ここで注意すべきこととしては、$\delta_k$は最大ステア角度$\delta_{\max}$以内の値を取る。曲率$\kappa_k$から計算された$\delta_{\mathrm{ref}, k}$が最大ステア角度$\delta_{\max}$より大きいときに$\delta_{\mathrm{ref}, k}$をそのまま使用して線形化すると、$\Delta \delta_k = \delta - \delta_{\mathrm{ref}, k} = \delta_{\max} - \delta_{\mathrm{ref}, k}$となり、$\Delta \delta_k$の絶対値が大きくなる。すなわち、$\delta_{\mathrm{ref}, k}$にも最大ステア角度制限を適用する必要がある。 + +$$ +\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})$は$v$を$v_{\min}$から$v_{\max}$の範囲内に丸める関数である。 + +この$\delta_{\mathrm{ref}, k}$を介して$\tan \delta_k$を線形な式で近似する。 + +$$ +\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} +$$ + +以上の線形化を踏まえ、誤差ダイナミクスは以下のように線形な行列演算で記述できる。 + +$$ +\begin{align} + \begin{pmatrix} + y_{k+1} \\ + \theta_{k+1} \\ + \delta_{k+1} + \end{pmatrix} + = + \begin{pmatrix} + 1 & v dt & 0 \\ + 0 & 1 & \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ + 0 & 0 & 1 - \frac{dt}{\tau} + \end{pmatrix} + \begin{pmatrix} + y_k \\ + \theta_k \\ + \delta_k + \end{pmatrix} + + + \begin{pmatrix} + 0 \\ + 0 \\ + \frac{dt}{\tau} + \end{pmatrix} + \delta_{des} + + + \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\\ + 0 + \end{pmatrix} +\end{align} +$$ + +平滑化と経路追従のための目的関数は以下で表され、 + +$$ +\begin{align} +J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) \\ & = 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 +\end{align} +$$ + +前述の通り、車両が障害物・道路境界に侵入しない条件はスラック変数を用いてソフト制約として表されている。 +車両の後輪位置、前輪位置、およびその中心位置における障害物・道路境界までの距離をそれぞれ$y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$とする。 +ここでそれぞれに対するスラック変数 $\lambda_{\mathrm{base}}, \lambda_{\mathrm{top}}, \lambda_{\mathrm{mid}}$を定義し、 + +$$ +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} +$$ + +$y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$は$y_k$の 1 次式として表現できるので、このソフト制約の目的関数は、以下のように記述できる。 + +$$ +\begin{align} +J_2 & (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) \\ & = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k}^2 + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k}^2 + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k}^2 +\end{align} +$$ + +スラック変数も二次計画法の設計変数となり、全ての設計変数をまとめたベクトル$\boldsymbol{x}$を定義する。 + +$$ +\begin{align} +\boldsymbol{x} = +\begin{pmatrix} +... & y_k & \lambda_{\mathrm{base}, k} & \lambda_{\mathrm{top}, k} & \lambda_{\mathrm{mid}, k} & ... +\end{pmatrix}^T +\end{align} +$$ + +これらの 2 つの目的関数の和を目的関数とする。 + +$$ +\begin{align} +\min_{\boldsymbol{x}} J (\boldsymbol{x}) = \min_{\boldsymbol{x}} J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) + J_2 (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) +\end{align} +$$ + +前述の通り、真にハードな制約条件は車両前方ある程度の距離$N_{fix}$までの経路点の状態は前回値になるような条件であり、以下のように記述できる。 + +$$ +\begin{align} +\delta_k = \delta_{k}^{\mathrm{prev}} (0 \leq i \leq N_{\mathrm{fix}}) +\end{align} +$$ + +であり、これらを以下のような二次計画法の係数行列に変換して二次計画法を解く + +$$ +\begin{align} +\min_{\boldsymbol{x}} \ & \frac{1}{2} \boldsymbol{x}^T \boldsymbol{P} \boldsymbol{x} + \boldsymbol{q} \boldsymbol{x} \\ +\mathrm{s.t.} \ & \boldsymbol{b}_l \leq \boldsymbol{A} \boldsymbol{x} \leq \boldsymbol{b}_u +\end{align} +$$ + +## Limitation + +- カーブ時に外側に膨らんだ経路を返す +- behavior_path_planner と obstacle_avoidance_planner の経路計画の役割分担がはっきりと決まっていない + - behavior_path_planner 側で回避する場合と、obstacle_avoidance_planner で回避する場合がある +- behavior_path_planner から来る path が道路壁に衝突していると、大きく外側に膨れた trajectory を計画する (柏の葉のカーブで顕著) +- 計算負荷が高い + +## How to debug + +obstacle_avoidance_planner` から出力される debug 用の topic について説明する。 + +- **interpolated_points_marker** + - obstacle avoidance planner への入力経路をリサンプルしたもの。この経路が道路内に収まっているか（道路内にあることが必須ではない）、十分になめらかか（ある程度滑らかでないとロジックが破綻する）、などを確認する。 + +![interpolated_points_marker](./media/interpolated_points_marker.png) + +- **smoothed_points_text** + - Elastic Band の計算結果。点群ではなく小さい数字が描画される。平滑化されたこの経路が道路内からはみ出ていないか、歪んでいないかなどを確認。 + +![smoothed_points_text](./media/smoothed_points_text.png) + +- **(base/top/mid)\_bounds_line** + - 壁との衝突判定における横方向の道路境界までの距離（正確には - vehicle_width / 2.0）。 + - 車両の 3 箇所（base, top, mid）で衝突判定を行っており、3 つのマーカーが存在する。 + - 黄色い線の各端点から道路境界までの距離が車幅の半分くらいであれば異常なし（ここがおかしい場合はロジック異常）。 + +![bounds_line](./media/bounds_line.png) + +- **optimized_points_marker** + - MPT の計算結果。道路からはみ出ていないか、振動していないかなどを確認 + +![optimized_points_marker](./media/optimized_points_marker.png) + +- **Trajectory with footprint** + - TrajectoryFootprint の rviz_plugin を用いて経路上の footprint を描画することが可能。これを用いて obstacle_avoidance_planner への入出力の footprint、経路に収まっているかどうか等を確認する。 + +![trajectory_with_footprint](./media/trajectory_with_footprint.png) + +- **Drivable Area** + - obstacle avoidance への入力の走行可能領域を表示する。Drivable Area の生成に不具合があると生成経路が歪む可能性がある。 + - topic 名：`/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area` + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![drivable_area](./media/drivable_area.png) + +- **area_with_objects** + - 入力された走行可能領域から障害物を取り除いた後の、走行可能領域 + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![area_with_objects](./media/area_with_objects.png) + +- **object_clearance_map** + - 回避対象の障害物からの距離を可視化したもの。 + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![object_clearance_map](./media/object_clearance_map.png) + +- **clearance_map** + - 入力された走行可能領域からの距離を可視化したもの。 + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![clearance_map](./media/clearance_map.png) diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index b76c6db96f215..c58292c667327 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -21,6 +21,7 @@ std_msgs tf2_ros tier4_autoware_utils + tier4_debug_msgs tier4_planning_msgs vehicle_info_util visualization_msgs diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp new file mode 100644 index 0000000000000..dafc2d5881b3e --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp @@ -0,0 +1,342 @@ +// Copyright 2021 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/costmap_generator.hpp" + +#include "obstacle_avoidance_planner/cv_utils.hpp" +#include "obstacle_avoidance_planner/utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace +{ +cv::Point toCVPoint(const geometry_msgs::msg::Point & p) +{ + cv::Point cv_point; + cv_point.x = p.x; + cv_point.y = p.y; + return cv_point; +} + +bool isAvoidingObjectType( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const TrajectoryParam & traj_param) +{ + if ( + (object.classification.at(0).label == object.classification.at(0).UNKNOWN && + traj_param.is_avoiding_unknown) || + (object.classification.at(0).label == object.classification.at(0).CAR && + traj_param.is_avoiding_car) || + (object.classification.at(0).label == object.classification.at(0).TRUCK && + traj_param.is_avoiding_truck) || + (object.classification.at(0).label == object.classification.at(0).BUS && + traj_param.is_avoiding_bus) || + (object.classification.at(0).label == object.classification.at(0).BICYCLE && + traj_param.is_avoiding_bicycle) || + (object.classification.at(0).label == object.classification.at(0).MOTORCYCLE && + traj_param.is_avoiding_motorbike) || + (object.classification.at(0).label == object.classification.at(0).PEDESTRIAN && + traj_param.is_avoiding_pedestrian)) { + return true; + } + return false; +} + +bool arePointsInsideDriveableArea( + const std::vector & image_points, const cv::Mat & clearance_map) +{ + bool points_inside_area = false; + for (const auto & image_point : image_points) { + const float clearance = + clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; + if (clearance > 0) { + points_inside_area = true; + } + } + return points_inside_area; +} + +bool isAvoidingObject( + const PolygonPoints & polygon_points, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info, + const std::vector & path_points, + const TrajectoryParam & traj_param) +{ + if (path_points.empty()) { + return false; + } + if (!isAvoidingObjectType(object, traj_param)) { + return false; + } + const auto image_point = geometry_utils::transformMapToOptionalImage( + object.kinematics.initial_pose_with_covariance.pose.position, map_info); + if (!image_point) { + return false; + } + + const int nearest_idx = tier4_autoware_utils::findNearestIndex( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + const auto nearest_path_point = path_points[nearest_idx]; + const auto rel_p = geometry_utils::transformToRelativeCoordinate2D( + object.kinematics.initial_pose_with_covariance.pose.position, nearest_path_point.pose); + // skip object located back the beginning of path points + if (nearest_idx == 0 && rel_p.x < 0) { + return false; + } + + /* + const float object_clearance_from_road = + clearance_map.ptr( + static_cast(image_point.get().y))[static_cast(image_point.get().x)] * + map_info.resolution; + */ + const geometry_msgs::msg::Vector3 twist = + object.kinematics.initial_twist_with_covariance.twist.linear; + const double vel = std::sqrt(twist.x * twist.x + twist.y * twist.y + twist.z * twist.z); + /* + const auto nearest_path_point_image = + geometry_utils::transformMapToOptionalImage(nearest_path_point.pose.position, map_info); + if (!nearest_path_point_image) { + return false; + } + const float nearest_path_point_clearance = + clearance_map.ptr(static_cast( + nearest_path_point_image.get().y))[static_cast(nearest_path_point_image.get().x)] * + map_info.resolution; + */ + const double lateral_offset_to_path = tier4_autoware_utils::calcLateralOffset( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + if ( + // nearest_path_point_clearance - traj_param.center_line_width * 0.5 < + // object_clearance_from_road || + std::abs(lateral_offset_to_path) < traj_param.center_line_width * 0.5 || + vel > traj_param.max_avoiding_objects_velocity_ms || + !arePointsInsideDriveableArea(polygon_points.points_in_image, clearance_map)) { + return false; + } + return true; +} +} // namespace + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const cv::Point & p) +{ + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x; + geom_p.y = p.y; + + return geom_p; +} +} // namespace tier4_autoware_utils + +CVMaps CostmapGenerator::getMaps( + const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & objects, + const TrajectoryParam & traj_param, std::shared_ptr debug_data_ptr) +{ + stop_watch_.tic(__func__); + + // make cv_maps + CVMaps cv_maps; + + cv_maps.drivable_area = getDrivableAreaInCV(path.drivable_area, debug_data_ptr); + cv_maps.clearance_map = getClearanceMap(cv_maps.drivable_area, debug_data_ptr); + + std::vector debug_avoiding_objects; + cv::Mat objects_image = drawObstaclesOnImage( + enable_avoidance, objects, path.points, path.drivable_area.info, cv_maps.drivable_area, + cv_maps.clearance_map, traj_param, &debug_avoiding_objects, debug_data_ptr); + + cv_maps.area_with_objects_map = + getAreaWithObjects(cv_maps.drivable_area, objects_image, debug_data_ptr); + cv_maps.only_objects_clearance_map = getClearanceMap(objects_image, debug_data_ptr); + cv_maps.map_info = path.drivable_area.info; + + // debug data + debug_data_ptr->clearance_map = cv_maps.clearance_map; + debug_data_ptr->only_object_clearance_map = cv_maps.only_objects_clearance_map; + debug_data_ptr->area_with_objects_map = cv_maps.area_with_objects_map; + debug_data_ptr->avoiding_objects = debug_avoiding_objects; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return cv_maps; +} + +cv::Mat CostmapGenerator::getDrivableAreaInCV( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + cv::Mat drivable_area = cv::Mat(occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1); + + drivable_area.forEach([&](unsigned char & value, const int * position) -> void { + cv_utils::getOccupancyGridValue(occupancy_grid, position[0], position[1], value); + }); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return drivable_area; +} + +cv::Mat CostmapGenerator::getClearanceMap( + const cv::Mat & drivable_area, std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + cv::Mat clearance_map; + cv::distanceTransform(drivable_area, clearance_map, cv::DIST_L2, 5); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return clearance_map; +} + +cv::Mat CostmapGenerator::drawObstaclesOnImage( + const bool enable_avoidance, + const std::vector & objects, + const std::vector & path_points, + const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, + const cv::Mat & clearance_map, const TrajectoryParam & traj_param, + std::vector * debug_avoiding_objects, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + std::vector path_points_inside_area; + for (const auto & point : path_points) { + std::vector points; + geometry_msgs::msg::Point image_point; + if (!geometry_utils::transformMapToImage(point.pose.position, map_info, image_point)) { + continue; + } + const float clearance = + clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; + if (clearance < 1e-5) { + continue; + } + path_points_inside_area.push_back(point); + } + + // NOTE: objects image is too sparse so that creating cost map is heavy. + // Then, objects image is created by filling dilated drivable area, + // instead of "cv::Mat objects_image = cv::Mat::ones(clearance_map.rows, clearance_map.cols, + // CV_8UC1) * 255;". + constexpr double dilate_margin = 1.0; + cv::Mat objects_image; + const int dilate_size = static_cast( + (1.8 + dilate_margin) / + (std::sqrt(2) * map_info.resolution)); // TODO(murooka) use clearance_from_object + cv::dilate(drivable_area, objects_image, cv::Mat(), cv::Point(-1, -1), dilate_size); + + if (!enable_avoidance) { + return objects_image; + } + + // fill object + std::vector> cv_polygons; + std::vector> obj_cog_info; + std::vector obj_positions; + for (const auto & object : objects) { + const PolygonPoints polygon_points = cv_polygon_utils::getPolygonPoints(object, map_info); + if (isAvoidingObject( + polygon_points, object, clearance_map, map_info, path_points_inside_area, traj_param)) { + const double lon_dist_to_path = tier4_autoware_utils::calcSignedArcLength( + path_points, 0, object.kinematics.initial_pose_with_covariance.pose.position); + const double lat_dist_to_path = tier4_autoware_utils::calcLateralOffset( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + obj_cog_info.push_back({lon_dist_to_path, lat_dist_to_path}); + obj_positions.push_back(object.kinematics.initial_pose_with_covariance.pose.position); + + cv_polygons.push_back(cv_polygon_utils::getCVPolygon( + object, polygon_points, path_points_inside_area, clearance_map, map_info)); + debug_avoiding_objects->push_back(object); + } + } + for (const auto & polygon : cv_polygons) { + cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); + } + + // fill between objects in the same side + const auto get_closest_obj_point = [&](size_t idx) { + const auto & path_point = + path_points.at(tier4_autoware_utils::findNearestIndex(path_points, obj_positions.at(idx))); + double min_dist = std::numeric_limits::min(); + size_t min_idx = 0; + for (size_t p_idx = 0; p_idx < cv_polygons.at(idx).size(); ++p_idx) { + const double dist = + tier4_autoware_utils::calcDistance2d(cv_polygons.at(idx).at(p_idx), path_point); + if (dist < min_dist) { + min_dist = dist; + min_idx = p_idx; + } + } + + geometry_msgs::msg::Point geom_point; + geom_point.x = cv_polygons.at(idx).at(min_idx).x; + geom_point.y = cv_polygons.at(idx).at(min_idx).y; + return geom_point; + }; + + std::vector> cv_between_polygons; + for (size_t i = 0; i < obj_positions.size(); ++i) { + for (size_t j = i + 1; j < obj_positions.size(); ++j) { + const auto & obj_info1 = obj_cog_info.at(i); + const auto & obj_info2 = obj_cog_info.at(j); + + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lat"), obj_info1.at(1) << " " << obj_info2.at(1)); + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lon"), obj_info1.at(0) << " " << obj_info2.at(0)); + + constexpr double max_lon_dist_to_convex_obstacles = 30.0; + if ( + obj_info1.at(1) * obj_info2.at(1) < 0 || + std::abs(obj_info1.at(0) - obj_info2.at(0)) > max_lon_dist_to_convex_obstacles) { + continue; + } + + std::vector cv_points; + cv_points.push_back(toCVPoint(obj_positions.at(i))); + cv_points.push_back(toCVPoint(get_closest_obj_point(i))); + cv_points.push_back(toCVPoint(get_closest_obj_point(j))); + cv_points.push_back(toCVPoint(obj_positions.at(j))); + + cv_between_polygons.push_back(cv_points); + } + } + /* + for (const auto & polygon : cv_between_polygons) { + cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); + } + */ + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + + return objects_image; +} + +cv::Mat CostmapGenerator::getAreaWithObjects( + const cv::Mat & drivable_area, const cv::Mat & objects_image, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + cv::Mat area_with_objects = cv::min(objects_image, drivable_area); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return area_with_objects; +} diff --git a/planning/obstacle_avoidance_planner/src/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/cv_utils.cpp new file mode 100644 index 0000000000000..a73a5c70fdeb3 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/cv_utils.cpp @@ -0,0 +1,463 @@ +// 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/cv_utils.hpp" + +#include "obstacle_avoidance_planner/utils.hpp" +#include "tf2/utils.h" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include + +namespace +{ +boost::optional getDistance( + const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & map_info) +{ + const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); + if (!image_point) { + return boost::none; + } + const float clearance = clearance_map.ptr(static_cast( + image_point.get().y))[static_cast(image_point.get().x)] * + map_info.resolution; + return clearance; +} + +bool isOutsideDrivableArea( + const geometry_msgs::msg::Point & pos, const cv::Mat & road_clearance_map, + const nav_msgs::msg::MapMetaData & map_info, const double max_dist) +{ + const auto dist = getDistance(road_clearance_map, pos, map_info); + if (dist) { + return dist.get() < max_dist; + } + + return false; +} +} // namespace + +namespace cv_utils +{ +void getOccupancyGridValue( + const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value) +{ + int i_flip = og.info.width - i - 1; + int j_flip = og.info.height - j - 1; + if (og.data[i_flip + j_flip * og.info.width] > 0) { + value = 0; + } else { + value = 255; + } +} + +void putOccupancyGridValue( + nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value) +{ + int i_flip = og.info.width - i - 1; + int j_flip = og.info.height - j - 1; + og.data[i_flip + j_flip * og.info.width] = value; +} +} // namespace cv_utils + +namespace cv_polygon_utils +{ +PolygonPoints getPolygonPoints( + const std::vector & points, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + for (const auto & point : points) { + const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); + if (image_point) { + points_in_image.push_back(image_point.get()); + points_in_map.push_back(point); + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +PolygonPoints getPolygonPoints( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + PolygonPoints polygon_points; + if (object.shape.type == object.shape.BOUNDING_BOX) { + polygon_points = getPolygonPointsFromBB(object, map_info); + } else if (object.shape.type == object.shape.CYLINDER) { + polygon_points = getPolygonPointsFromCircle(object, map_info); + } else if (object.shape.type == object.shape.POLYGON) { + polygon_points = getPolygonPointsFromPolygon(object, map_info); + } + return polygon_points; +} + +PolygonPoints getPolygonPointsFromBB( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + const double dim_x = object.shape.dimensions.x; + const double dim_y = object.shape.dimensions.y; + const std::vector rel_x = {0.5 * dim_x, 0.5 * dim_x, -0.5 * dim_x, -0.5 * dim_x}; + const std::vector rel_y = {0.5 * dim_y, -0.5 * dim_y, -0.5 * dim_y, 0.5 * dim_y}; + const geometry_msgs::msg::Pose object_pose = object.kinematics.initial_pose_with_covariance.pose; + for (size_t i = 0; i < rel_x.size(); i++) { + geometry_msgs::msg::Point rel_point; + rel_point.x = rel_x[i]; + rel_point.y = rel_y[i]; + auto abs_point = geometry_utils::transformToAbsoluteCoordinate2D(rel_point, object_pose); + geometry_msgs::msg::Point image_point; + if (geometry_utils::transformMapToImage(abs_point, map_info, image_point)) { + points_in_image.push_back(image_point); + points_in_map.push_back(abs_point); + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +PolygonPoints getPolygonPointsFromCircle( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + const double radius = object.shape.dimensions.x; + const geometry_msgs::msg::Point center = + object.kinematics.initial_pose_with_covariance.pose.position; + constexpr int num_sampling_points = 5; + for (int i = 0; i < num_sampling_points; ++i) { + std::vector deltas = {0, 1.0}; + for (const auto & delta : deltas) { + geometry_msgs::msg::Point point; + point.x = std::cos( + ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + + M_PI / static_cast(num_sampling_points)) * + (radius / 2.0) + + center.x; + point.y = std::sin( + ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + + M_PI / static_cast(num_sampling_points)) * + (radius / 2.0) + + center.y; + point.z = center.z; + geometry_msgs::msg::Point image_point; + if (geometry_utils::transformMapToImage(point, map_info, image_point)) { + points_in_image.push_back(image_point); + points_in_map.push_back(point); + } + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +PolygonPoints getPolygonPointsFromPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + for (const auto & polygon_p : object.shape.footprint.points) { + geometry_msgs::msg::Point rel_point; + rel_point.x = polygon_p.x; + rel_point.y = polygon_p.y; + geometry_msgs::msg::Point point = geometry_utils::transformToAbsoluteCoordinate2D( + rel_point, object.kinematics.initial_pose_with_covariance.pose); + const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); + if (image_point) { + points_in_image.push_back(image_point.get()); + points_in_map.push_back(point); + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +std::vector getCVPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const PolygonPoints & polygon_points, + const std::vector & path_points, + const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) +{ + const int nearest_idx = tier4_autoware_utils::findNearestIndex( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + const auto nearest_path_point = path_points[nearest_idx]; + if (path_points.empty()) { + return getDefaultCVPolygon(polygon_points.points_in_image); + } + return getExtendedCVPolygon( + polygon_points.points_in_image, polygon_points.points_in_map, nearest_path_point.pose, object, + clearance_map, map_info); +} + +std::vector getDefaultCVPolygon( + const std::vector & points_in_image) +{ + std::vector cv_polygon; + for (const auto & point : points_in_image) { + cv::Point image_point = cv::Point(point.x, point.y); + cv_polygon.push_back(image_point); + } + return cv_polygon; +} + +std::vector getExtendedCVPolygon( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info) +{ + const boost::optional optional_edges = getEdges( + points_in_image, points_in_map, nearest_path_point_pose, object, clearance_map, map_info); + if (!optional_edges) { + return getDefaultCVPolygon(points_in_image); + } + const Edges edges = optional_edges.get(); + + const int nearest_polygon_idx = + tier4_autoware_utils::findNearestIndex(points_in_image, edges.origin); + std::vector cv_polygon; + if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { + // make polygon only with edges and extended edges + } else if (edges.back_idx < nearest_polygon_idx) { + // back_idx -> nearest_idx -> frond_idx + if (edges.back_idx < edges.front_idx && nearest_polygon_idx < edges.front_idx) { + for (int i = edges.back_idx + 1; i < edges.front_idx; i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + // back_idx -> vector_front -> vector_back -> nearest_idx -> frond_idx + } else if (edges.back_idx < edges.front_idx && nearest_polygon_idx > edges.front_idx) { + for (int i = edges.back_idx - 1; i >= 0; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx + } else { + for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = 0; i < edges.front_idx; i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + } + } else { + // back_idx -> nearest_idx -> front_idx + if (edges.back_idx >= edges.front_idx && nearest_polygon_idx > edges.front_idx) { + for (int i = edges.back_idx - 1; i > edges.front_idx; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx + } else { + if (edges.back_idx >= edges.front_idx && nearest_polygon_idx < edges.front_idx) { + for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = 0; i < edges.front_idx; i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + } else { // back_idx -> vector_front -> vector_back -> nearest_idx -> front_idx + for (int i = edges.back_idx - 1; i >= 0; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + } + } + } + cv_polygon.push_back( + cv::Point(points_in_image[edges.front_idx].x, points_in_image[edges.front_idx].y)); + cv_polygon.push_back(cv::Point(edges.extended_front.x, edges.extended_front.y)); + cv_polygon.push_back(cv::Point(edges.extended_back.x, edges.extended_back.y)); + cv_polygon.push_back( + cv::Point(points_in_image[edges.back_idx].x, points_in_image[edges.back_idx].y)); + return cv_polygon; +} + +boost::optional getEdges( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info) +{ + // calculate perpendicular point to object along with path point orientation + const double yaw = tf2::getYaw(nearest_path_point_pose.orientation); + const Eigen::Vector2d rel_path_vec(std::cos(yaw), std::sin(yaw)); + const Eigen::Vector2d obj_vec( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + const double inner_product = rel_path_vec[0] * (obj_vec[0] - nearest_path_point_pose.position.x) + + rel_path_vec[1] * (obj_vec[1] - nearest_path_point_pose.position.y); + geometry_msgs::msg::Point origin; + origin.x = nearest_path_point_pose.position.x + rel_path_vec[0] * inner_product; + origin.y = nearest_path_point_pose.position.y + rel_path_vec[1] * inner_product; + const Eigen::Vector2d obj2origin(origin.x - obj_vec[0], origin.y - obj_vec[1]); + + // calculate origin for casting ray to edges + const auto path_point_image = + geometry_utils::transformMapToImage(nearest_path_point_pose.position, map_info); + constexpr double ray_origin_dist_scale = 1.0; + const float clearance = clearance_map.ptr(static_cast( + path_point_image.y))[static_cast(path_point_image.x)] * + map_info.resolution * ray_origin_dist_scale; + const Eigen::Vector2d obj2ray_origin = obj2origin.normalized() * (obj2origin.norm() + clearance); + geometry_msgs::msg::Point ray_origin; + ray_origin.x = obj_vec[0] + obj2ray_origin[0]; + ray_origin.y = obj_vec[1] + obj2ray_origin[1]; + geometry_msgs::msg::Point ray_origin_image; + ray_origin_image = geometry_utils::transformMapToImage(ray_origin, map_info); + + double min_cos = std::numeric_limits::max(); + double max_cos = std::numeric_limits::lowest(); + const double path_yaw = tf2::getYaw(nearest_path_point_pose.orientation); + const double dx1 = std::cos(path_yaw); + const double dy1 = std::sin(path_yaw); + const Eigen::Vector2d path_point_vec(dx1, dy1); + const double path_point_vec_norm = path_point_vec.norm(); + Edges edges; + for (size_t i = 0; i < points_in_image.size(); i++) { + const double dx2 = points_in_map[i].x - ray_origin.x; + const double dy2 = points_in_map[i].y - ray_origin.y; + const Eigen::Vector2d path_point2point(dx2, dy2); + const double inner_product = path_point_vec.dot(path_point2point); + const double cos = inner_product / (path_point_vec_norm * path_point2point.norm()); + if (cos > max_cos) { + max_cos = cos; + edges.front_idx = i; + } + if (cos < min_cos) { + min_cos = cos; + edges.back_idx = i; + } + } + + const double max_sin = std::sqrt(1 - max_cos * max_cos); + const double min_sin = std::sqrt(1 - min_cos * min_cos); + const Eigen::Vector2d point2front_edge( + points_in_image[edges.front_idx].x - ray_origin_image.x, + points_in_image[edges.front_idx].y - ray_origin_image.y); + const Eigen::Vector2d point2back_edge( + points_in_image[edges.back_idx].x - ray_origin_image.x, + points_in_image[edges.back_idx].y - ray_origin_image.y); + const Eigen::Vector2d point2extended_front_edge = + point2front_edge.normalized() * (clearance * 2 / max_sin) * (1 / map_info.resolution); + const Eigen::Vector2d point2extended_back_edge = + point2back_edge.normalized() * (clearance * 2 / min_sin) * (1 / map_info.resolution); + + const double dist2extended_front_edge = point2extended_front_edge.norm() * map_info.resolution; + const double dist2front_edge = point2front_edge.norm() * map_info.resolution; + const double dist2extended_back_edge = point2extended_back_edge.norm() * map_info.resolution; + const double dist2back_edge = point2back_edge.norm() * map_info.resolution; + if ( + dist2extended_front_edge < clearance * 2 || dist2extended_back_edge < clearance * 2 || + dist2front_edge > dist2extended_front_edge || dist2back_edge > dist2extended_back_edge) { + return boost::none; + } + geometry_msgs::msg::Point extended_front; + geometry_msgs::msg::Point extended_back; + extended_front.x = point2extended_front_edge(0) + ray_origin_image.x; + extended_front.y = point2extended_front_edge(1) + ray_origin_image.y; + extended_back.x = point2extended_back_edge(0) + ray_origin_image.x; + extended_back.y = point2extended_back_edge(1) + ray_origin_image.y; + edges.extended_front = extended_front; + edges.extended_back = extended_back; + edges.origin = ray_origin_image; + return edges; +} +} // namespace cv_polygon_utils + +namespace cv_drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const VehicleParam & vehicle_param) +{ + const double half_width = vehicle_param.width / 2.0; + 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, -half_width, 0.0).position; + const auto top_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0).position; + const auto bottom_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0).position; + const auto bottom_left_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0).position; + + constexpr double epsilon = 1e-8; + const bool out_top_left = + isOutsideDrivableArea(top_left_pos, road_clearance_map, map_info, epsilon); + const bool out_top_right = + isOutsideDrivableArea(top_right_pos, road_clearance_map, map_info, epsilon); + const bool out_bottom_left = + isOutsideDrivableArea(bottom_left_pos, road_clearance_map, map_info, epsilon); + const bool out_bottom_right = + isOutsideDrivableArea(bottom_right_pos, road_clearance_map, map_info, epsilon); + + if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { + return true; + } + + return false; +} + +bool isOutsideDrivableAreaFromCirclesFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const std::vector vehicle_circle_longitudinal_offsets, const double vehicle_circle_radius) +{ + for (const double offset : vehicle_circle_longitudinal_offsets) { + const auto avoiding_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, offset, 0.0, 0.0).position; + + const bool outside_drivable_area = + isOutsideDrivableArea(avoiding_pos, road_clearance_map, map_info, vehicle_circle_radius); + if (outside_drivable_area) { + return true; + } + } + + return false; +} + +} // namespace cv_drivable_area_utils diff --git a/planning/obstacle_avoidance_planner/src/debug.cpp b/planning/obstacle_avoidance_planner/src/debug.cpp index 7c0036f0b03ea..5b591c6412224 100644 --- a/planning/obstacle_avoidance_planner/src/debug.cpp +++ b/planning/obstacle_avoidance_planner/src/debug.cpp @@ -18,7 +18,6 @@ #include "obstacle_avoidance_planner/marker_helper.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/debug_visualization.cpp b/planning/obstacle_avoidance_planner/src/debug_visualization.cpp new file mode 100644 index 0000000000000..86b3cc43420c0 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/debug_visualization.cpp @@ -0,0 +1,826 @@ +// 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/debug_visualization.hpp" + +#include "obstacle_avoidance_planner/cv_utils.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "obstacle_avoidance_planner/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 half_width = vehicle_param.width / 2.0; + 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, -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()); + + 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 half_width = vehicle_param.width / 2.0; + 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, half_width, 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, + const SequentialBoundsCandidates & sequential_bounds_candidates, const double r, const double g, + const double b, const double vehicle_circle_radius, const size_t sampling_num) +{ + const auto current_time = rclcpp::Clock().now(); + visualization_msgs::msg::MarkerArray msg; + + int unique_id = 0; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "bounds_candidates", 0, + visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.05, 0, 0), + createMarkerColor(r + 0.5, g, b, 0.3)); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + + for (size_t p_idx = 0; p_idx < sequential_bounds_candidates.size(); ++p_idx) { + if (p_idx % sampling_num != 0) { + continue; + } + + const auto & bounds_candidates = sequential_bounds_candidates.at(p_idx); + for (size_t b_idx = 0; b_idx < bounds_candidates.size(); ++b_idx) { + const auto & bounds_candidate = bounds_candidates.at(b_idx); + + marker.id = unique_id++; + geometry_msgs::msg::Pose pose; + pose.position = ref_points.at(p_idx).p; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(p_idx).yaw); + + const double lb_y = bounds_candidate.lower_bound - vehicle_circle_radius; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + marker.points.push_back(lb); + + const double ub_y = bounds_candidate.upper_bound + vehicle_circle_radius; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + marker.points.push_back(ub); + } + } + + msg.markers.push_back(marker); + return msg; +} +*/ + +visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray( + const std::vector & ref_points, const double r, const double g, const double b, + const double vehicle_circle_radius, const size_t sampling_num) +{ + const auto current_time = rclcpp::Clock().now(); + visualization_msgs::msg::MarkerArray msg; + + if (ref_points.empty()) return msg; + + 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 - vehicle_circle_radius; + 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 + vehicle_circle_radius; + 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 double vehicle_circle_radius, 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; + + 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, vehicle_circle_radius, 0.0).position; + const auto lb = + tier4_autoware_utils::calcOffsetPose(pose, 0.0, -vehicle_circle_radius, 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 std::vector & vehicle_circle_longitudinal_offsets, + const double vehicle_circle_radius, const std::string & ns, const double r, const double g, + const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + size_t id = 0; + for (const double offset : vehicle_circle_longitudinal_offsets) { + 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_radius * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radius * std::sin(edge_angle); + + 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, + const std::vector & vehicle_circle_longitudinal_offsets, + const double vehicle_circle_radius, 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 (const double offset : vehicle_circle_longitudinal_offsets) { + 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_radius * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radius * std::sin(edge_angle); + + marker.points.push_back(edge_pos); + } + + msg.markers.push_back(marker); + ++id; + } + } + 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; + + 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.text = "drivable area"; + + visualization_msgs::msg::MarkerArray msg; + msg.markers.push_back(marker); + + return msg; +} +} // namespace + +namespace debug_visualization +{ +visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( + const std::shared_ptr debug_data_ptr, + const std::vector & optimized_points, + const VehicleParam & vehicle_param, const bool is_showing_debug_detail) +{ + // virtual wall + visualization_msgs::msg::MarkerArray vis_marker_array; + if (debug_data_ptr->stop_pose_by_drivable_area) { + const auto virtual_wall_pose = + getVirtualWallPose(debug_data_ptr->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); + } + + if (is_showing_debug_detail) { + const auto points_marker_array = getDebugPointsMarkers( + debug_data_ptr->interpolated_points, optimized_points, debug_data_ptr->straight_points, + debug_data_ptr->fixed_points, debug_data_ptr->non_fixed_points); + + const auto constrain_marker_array = + getDebugConstrainMarkers(debug_data_ptr->constrain_rectangles, "constrain_rect"); + + appendMarkerArray(points_marker_array, &vis_marker_array); + appendMarkerArray(constrain_marker_array, &vis_marker_array); + + appendMarkerArray( + getRectanglesNumMarkerArray( + debug_data_ptr->mpt_traj, vehicle_param, "num_vehicle_footprint", 0.99, 0.99, 0.2), + &vis_marker_array); + + appendMarkerArray( + getPointsTextMarkerArray(debug_data_ptr->eb_traj, "eb_traj_text", 0.99, 0.99, 0.2), + &vis_marker_array); + } + + // avoiding objects + appendMarkerArray( + getObjectsMarkerArray(debug_data_ptr->avoiding_objects, "avoiding_objects", 0.99, 0.99, 0.2), + &vis_marker_array); + // mpt footprints + appendMarkerArray( + getRectanglesMarkerArray( + debug_data_ptr->mpt_traj, vehicle_param, "mpt_footprints", 0.99, 0.99, 0.2, + debug_data_ptr->mpt_visualize_sampling_num), + &vis_marker_array); + // bounds + appendMarkerArray( + getBoundsLineMarkerArray( + debug_data_ptr->ref_points, 0.99, 0.99, 0.2, debug_data_ptr->vehicle_circle_radius, + debug_data_ptr->mpt_visualize_sampling_num), + &vis_marker_array); + + /* + // bounds candidates + appendMarkerArray( + getBoundsCandidatesLineMarkerArray( + debug_data_ptr->ref_points, debug_data_ptr->sequential_bounds_candidates, 0.2, 0.99, 0.99, + debug_data_ptr->vehicle_circle_radius, debug_data_ptr->mpt_visualize_sampling_num), + &vis_marker_array); + */ + + // vehicle circle line + appendMarkerArray( + getVehicleCircleLineMarkerArray( + debug_data_ptr->vehicle_circles_pose, debug_data_ptr->vehicle_circle_radius, + debug_data_ptr->mpt_visualize_sampling_num, "vehicle_circle_lines", 0.99, 0.99, 0.2), + &vis_marker_array); + + // lateral error line + appendMarkerArray( + getLateralErrorsLineMarkerArray( + debug_data_ptr->mpt_ref_poses, debug_data_ptr->lateral_errors, + debug_data_ptr->mpt_visualize_sampling_num, "lateral_errors", 0.1, 0.1, 0.8), + &vis_marker_array); + + // current vehicle circles + appendMarkerArray( + getCurrentVehicleCirclesMarkerArray( + debug_data_ptr->current_ego_pose, debug_data_ptr->vehicle_circle_longitudinal_offsets, + debug_data_ptr->vehicle_circle_radius, "current_vehicle_circles", 1.0, 0.3, 0.3), + &vis_marker_array); + + // vehicle circles + appendMarkerArray( + getVehicleCirclesMarkerArray( + debug_data_ptr->mpt_traj, debug_data_ptr->vehicle_circle_longitudinal_offsets, + debug_data_ptr->vehicle_circle_radius, debug_data_ptr->mpt_visualize_sampling_num, + "vehicle_circles", 1.0, 0.3, 0.3), + &vis_marker_array); + + return vis_marker_array; +} + +nav_msgs::msg::OccupancyGrid getDebugCostmap( + const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid) +{ + if (clearance_map.empty()) return nav_msgs::msg::OccupancyGrid(); + + cv::Mat tmp; + clearance_map.copyTo(tmp); + cv::normalize(tmp, tmp, 0, 255, cv::NORM_MINMAX, CV_8UC1); + nav_msgs::msg::OccupancyGrid clearance_map_in_og = occupancy_grid; + tmp.forEach([&](const unsigned char & value, const int * position) -> void { + cv_utils::putOccupancyGridValue(clearance_map_in_og, position[0], position[1], value); + }); + return clearance_map_in_og; +} +} // namespace debug_visualization diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index 57dcad29a7c84..6743bc7884e1a 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -14,25 +14,9 @@ #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" +#include "obstacle_avoidance_planner/utils.hpp" -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include +#include "geometry_msgs/msg/vector3.hpp" #include #include @@ -41,66 +25,46 @@ #include EBPathOptimizer::EBPathOptimizer( - const bool is_showing_debug_info, const QPParam qp_param, const TrajectoryParam traj_pram, - const ConstrainParam constrain_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param) -: logger_ros_clock_(RCL_ROS_TIME), - is_showing_debug_info_(is_showing_debug_info), + 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_(qp_param), - traj_param_(traj_pram), - constrain_param_(constrain_param), + qp_param_(eb_param.qp_param), + traj_param_(traj_param), + eb_param_(eb_param), vehicle_param_(vehicle_param) { - geometry_msgs::msg::Vector3 keep_space_shape; - keep_space_shape.x = constrain_param_.keep_space_shape_x; - keep_space_shape.y = constrain_param_.keep_space_shape_y; - keep_space_shape_ptr_ = std::make_unique(keep_space_shape); - mpt_optimizer_ptr_ = std::make_unique( - is_showing_debug_info, qp_param, traj_pram, constrain_param, vehicle_param, mpt_param); - initializeSolver(); -} - -EBPathOptimizer::~EBPathOptimizer() {} - -void EBPathOptimizer::initializeSolver() -{ - Eigen::MatrixXd p = makePMatrix(); + const Eigen::MatrixXd p = makePMatrix(); default_a_matrix_ = makeAMatrix(); - std::vector q(traj_param_.num_sampling_points * 2, 0.0); - std::vector lower_bound(traj_param_.num_sampling_points * 2, 0.0); - std::vector upper_bound(traj_param_.num_sampling_points * 2, 0.0); + 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); - ex_osqp_solver_ptr_ = std::make_unique( - p, default_a_matrix_, q, lower_bound, upper_bound, qp_param_.eps_abs); - ex_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - ex_osqp_solver_ptr_->updateMaxIter(qp_param_.max_iteration); - vis_osqp_solver_ptr_ = std::make_unique( - p, default_a_matrix_, q, lower_bound, upper_bound, qp_param_.eps_abs); - vis_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - vis_osqp_solver_ptr_->updateMaxIter(qp_param_.max_iteration); } -/* make positive semidefinite matrix for objective function - reference: https://ieeexplore.ieee.org/document/7402333 */ +// make positive semidefinite matrix for objective function +// reference: https://ieeexplore.ieee.org/document/7402333 Eigen::MatrixXd EBPathOptimizer::makePMatrix() { - Eigen::MatrixXd P = - Eigen::MatrixXd::Zero(traj_param_.num_sampling_points * 2, traj_param_.num_sampling_points * 2); - for (int r = 0; r < traj_param_.num_sampling_points * 2; r++) { - for (int c = 0; c < traj_param_.num_sampling_points * 2; c++) { + 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; + P(r, c) = 6.0; } else if (std::abs(c - r) == 1) { - P(r, c) = -4; + P(r, c) = -4.0; } else if (std::abs(c - r) == 2) { - P(r, c) = 1; + P(r, c) = 1.0; } else { - P(r, c) = 0; + P(r, c) = 0.0; } } } @@ -110,37 +74,33 @@ Eigen::MatrixXd EBPathOptimizer::makePMatrix() // make default linear constrain matrix Eigen::MatrixXd EBPathOptimizer::makeAMatrix() { - Eigen::MatrixXd A = Eigen::MatrixXd::Identity( - traj_param_.num_sampling_points * 2, traj_param_.num_sampling_points * 2); - for (int i = 0; i < traj_param_.num_sampling_points * 2; i++) { - if (i < traj_param_.num_sampling_points) { - A(i, i + traj_param_.num_sampling_points) = 1; + 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 - traj_param_.num_sampling_points) = 1; + A(i, i - num_points) = 1.0; } } return A; } -boost::optional EBPathOptimizer::generateOptimizedTrajectory( - const bool enable_avoidance, const geometry_msgs::msg::Pose & ego_pose, - const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, - const std::vector & objects, - DebugData * debug_data) +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, + std::shared_ptr debug_data_ptr) { - // processing drivable area - auto t_start1 = std::chrono::high_resolution_clock::now(); - CVMaps cv_maps = process_cv::getMaps(enable_avoidance, path, objects, traj_param_, debug_data); - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Processing driveable area time: = %f [ms]", elapsed_ms1); + stop_watch_.tic(__func__); + + current_ego_vel_ = current_ego_vel; // get candidate points for optimization - CandidatePoints candidate_points = getCandidatePoints( - ego_pose, path.points, prev_trajs, cv_maps.drivable_area, path.drivable_area.info, debug_data); + // 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_ptr); if (candidate_points.fixed_points.empty() && candidate_points.non_fixed_points.empty()) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, @@ -148,231 +108,115 @@ boost::optional EBPathOptimizer::generateOptimizedTrajectory( return boost::none; } - // get optimized smooth points - const auto opt_traj_points = getOptimizedTrajectory( - enable_avoidance, path, candidate_points, cv_maps.clearance_map, - cv_maps.only_objects_clearance_map, debug_data); - if (!opt_traj_points) { + // get optimized smooth points with elastic band + const auto eb_traj_points = getOptimizedTrajectory(path, candidate_points, debug_data_ptr); + if (!eb_traj_points) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, "return boost::none since smoothing failed"); return boost::none; } - const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - enable_avoidance, opt_traj_points.get(), path.points, prev_trajs, cv_maps, ego_pose, - debug_data); - if (!mpt_trajs) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "return boost::none since mpt failed"); - return boost::none; - } - - auto t_start2 = std::chrono::high_resolution_clock::now(); - std::vector extended_traj_points = - getExtendedOptimizedTrajectory(path.points, mpt_trajs.get().mpt, debug_data); - auto t_end2 = std::chrono::high_resolution_clock::now(); - float elapsed_ms2 = std::chrono::duration(t_end2 - t_start2).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Extending trajectory time: = %f [ms]", elapsed_ms2); - - Trajectories traj; - traj.smoothed_trajectory = opt_traj_points.get(); - traj.mpt_ref_points = mpt_trajs.get().ref_points; - traj.model_predictive_trajectory = mpt_trajs.get().mpt; - traj.extended_trajectory = extended_traj_points; - debug_data->smoothed_points = opt_traj_points.get(); - return traj; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return eb_traj_points; } boost::optional> EBPathOptimizer::getOptimizedTrajectory( - [[maybe_unused]] const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const CandidatePoints & candidate_points, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data) + const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, + std::shared_ptr debug_data_ptr) { + stop_watch_.tic(__func__); + // get constrain rectangles around each point - std::vector interpolated_points = util::getInterpolatedPoints( - candidate_points.fixed_points, candidate_points.non_fixed_points, - traj_param_.delta_arc_length_for_optimization); + 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 + const std::vector interpolated_points = + interpolation_utils::getInterpolatedPoints(full_points, eb_param_.delta_arc_length_for_eb); if (interpolated_points.empty()) { return boost::none; } - debug_data->interpolated_points = interpolated_points; + debug_data_ptr->interpolated_points = interpolated_points; + // number of optimizing points const int farthest_idx = std::min( - (traj_param_.num_sampling_points - 1), static_cast(interpolated_points.size() - 1)); + (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); - const int straight_line_idx = getStraightLineIdx( - interpolated_points, farthest_idx, only_objects_clearance_map, path.drivable_area.info, - debug_data->straight_points); + // 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_ptr->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); - auto t_start1 = std::chrono::high_resolution_clock::now(); + const auto rectangles = getConstrainRectangleVec( - path, padded_interpolated_points, num_fixed_points, farthest_idx, straight_line_idx, - clearance_map, only_objects_clearance_map); - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Make constrain rectangle time: = %f [ms]", elapsed_ms1); + 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, OptMode::Normal); - debug_data->smoothed_points = traj_points; - return traj_points; -} + const auto traj_points = + calculateTrajectory(padded_interpolated_points, rectangles.get(), farthest_idx, debug_data_ptr); -std::vector -EBPathOptimizer::getExtendedOptimizedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points, - DebugData * debug_data) -{ - if (static_cast(optimized_points.size()) <= traj_param_.num_fix_points_for_extending) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(10000).count(), "[Avoidance] Not extend trajectory"); - return std::vector{}; - } - const double accum_arc_length = util::getArcLength(optimized_points); - if ( - accum_arc_length > traj_param_.trajectory_length || - util::getLastExtendedPoint( - path_points.back(), optimized_points.back().pose, - traj_param_.delta_yaw_threshold_for_closest_point, - traj_param_.max_dist_for_extending_end_point)) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(10000).count(), "[Avoidance] Not extend trajectory"); - return std::vector{}; - } - const int default_idx = -1; - int begin_idx = util::getNearestIdx( - path_points, optimized_points.back().pose, default_idx, - traj_param_.delta_yaw_threshold_for_closest_point); - if (begin_idx == default_idx) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(10000).count(), - "[Avoidance] Not extend traj since could not find nearest idx from last opt point"); - return std::vector{}; - } - begin_idx = std::min( - begin_idx + traj_param_.num_offset_for_begin_idx, static_cast(path_points.size()) - 1); - const double extending_trajectory_length = traj_param_.trajectory_length - accum_arc_length; - const int end_extended_path_idx = - getEndPathIdx(path_points, begin_idx, extending_trajectory_length); - std::vector non_fixed_points; - for (int i = begin_idx; i <= end_extended_path_idx; i++) { - non_fixed_points.push_back(path_points[i].pose); - } - - std::vector fixed_points; - for (int i = traj_param_.num_fix_points_for_extending; i > 0; i--) { - fixed_points.push_back(optimized_points[optimized_points.size() - i].pose); - } - - const double delta_arc_length_for_extending_trajectory = std::fmax( - traj_param_.delta_arc_length_for_optimization, - static_cast(extending_trajectory_length / traj_param_.num_sampling_points)); - std::vector interpolated_points = util::getInterpolatedPoints( - fixed_points, non_fixed_points, delta_arc_length_for_extending_trajectory); - if (interpolated_points.empty()) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(1000).count(), - "[Avoidance] Not extend traj since empty interpolated points"); - return std::vector{}; - } - const int farthest_idx = std::min( - (traj_param_.num_sampling_points - 1), static_cast(interpolated_points.size() - 1)); - std::vector padded_interpolated_points = - getPaddedInterpolatedPoints(interpolated_points, farthest_idx); - const double arc_length = util::getArcLength(fixed_points); - const int num_fix_points = - static_cast(arc_length) / delta_arc_length_for_extending_trajectory; - std::vector constrain_rectangles = - getConstrainRectangleVec(path_points, padded_interpolated_points, num_fix_points, farthest_idx); - - const auto extended_traj_points = calculateTrajectory( - padded_interpolated_points, constrain_rectangles, farthest_idx, OptMode::Extending); - - const int default_extend_begin_idx = 0; - const int extend_begin_idx = util::getNearestIdxOverPoint( - extended_traj_points, optimized_points.back().pose, default_extend_begin_idx, - traj_param_.delta_yaw_threshold_for_closest_point); - - std::vector extended_traj; - for (std::size_t i = extend_begin_idx; i < extended_traj_points.size(); i++) { - extended_traj.push_back(extended_traj_points[i]); - } - - debug_data->fixed_points_for_extending = fixed_points; - debug_data->non_fixed_points_for_extending = non_fixed_points; - debug_data->constrain_rectangles_for_extending = constrain_rectangles; - debug_data->interpolated_points_for_extending = interpolated_points; - return extended_traj; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return traj_points; } std::vector EBPathOptimizer::calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, - const OptMode & opt_mode) + std::shared_ptr debug_data_ptr) { + stop_watch_.tic(__func__); + // update constrain for QP based on constrain rectangles - updateConstrain(padded_interpolated_points, constrain_rectangles, opt_mode); + updateConstrain(padded_interpolated_points, constrain_rectangles); // solve QP and get optimized trajectory - auto t_start2 = std::chrono::high_resolution_clock::now(); - std::vector optimized_points = solveQP(opt_mode); - auto t_end2 = std::chrono::high_resolution_clock::now(); - float elapsed_ms2 = std::chrono::duration(t_end2 - t_start2).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, "Optimization time: = %f [ms]", - elapsed_ms2); - auto traj_points = + std::vector optimized_points = solveQP(); + + const auto traj_points = convertOptimizedPointsToTrajectory(optimized_points, constrain_rectangles, farthest_idx); + + if (debug_data_ptr) { + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + } return traj_points; } -std::vector EBPathOptimizer::solveQP(const OptMode & opt_mode) +std::vector EBPathOptimizer::solveQP() { - std::vector optimized_points; - if (opt_mode == OptMode::Normal) { - osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs); - auto result = osqp_solver_ptr_->optimize(); - optimized_points = std::get<0>(result); - util::logOSQPSolutionStatus(std::get<3>(result)); - } else if (opt_mode == OptMode::Extending) { - ex_osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs_for_extending); - ex_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel_for_extending); - auto result = ex_osqp_solver_ptr_->optimize(); - optimized_points = std::get<0>(result); - util::logOSQPSolutionStatus(std::get<3>(result)); - } else if (opt_mode == OptMode::Visualizing) { - vis_osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs_for_visualizing); - vis_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel_for_visualizing); - auto result = vis_osqp_solver_ptr_->optimize(); - optimized_points = std::get<0>(result); - util::logOSQPSolutionStatus(std::get<3>(result)); - } + 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); + + utils::logOSQPSolutionStatus(std::get<3>(result)); + return optimized_points; } std::vector EBPathOptimizer::getFixedPoints( const geometry_msgs::msg::Pose & ego_pose, [[maybe_unused]] const std::vector & path_points, - const std::unique_ptr & prev_trajs, [[maybe_unused]] const cv::Mat & drivable_area, - [[maybe_unused]] const nav_msgs::msg::MapMetaData & map_info) + const std::unique_ptr & prev_trajs) { /* use of prev_traj_points(fine resolution) instead of prev_opt_traj(coarse resolution) stabilize trajectory's yaw*/ @@ -381,19 +225,25 @@ std::vector EBPathOptimizer::getFixedPoints( std::vector empty_points; return empty_points; } - const int default_idx = 0; - int begin_idx = util::getNearestIdx( - prev_trajs->smoothed_trajectory, ego_pose, default_idx, + const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + prev_trajs->smoothed_trajectory, ego_pose, std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); + const int begin_idx = opt_begin_idx ? *opt_begin_idx : 0; 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 + - traj_param_.forward_fixing_distance / traj_param_.delta_arc_length_for_trajectory), + 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++) { @@ -406,31 +256,33 @@ std::vector EBPathOptimizer::getFixedPoints( } } -CandidatePoints EBPathOptimizer::getCandidatePoints( +EBPathOptimizer::CandidatePoints EBPathOptimizer::getCandidatePoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info, DebugData * debug_data) + const std::unique_ptr & prev_trajs, std::shared_ptr debug_data_ptr) { const std::vector fixed_points = - getFixedPoints(ego_pose, path_points, prev_trajs, drivable_area, map_info); + getFixedPoints(ego_pose, path_points, prev_trajs); if (fixed_points.empty()) { CandidatePoints candidate_points = getDefaultCandidatePoints(path_points); return candidate_points; } - const int default_idx = -1; - int begin_idx = util::getNearestIdx( - path_points, fixed_points.back(), default_idx, + + // try to find non-fix points + const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + path_points, fixed_points.back(), std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); - if (begin_idx == default_idx) { + 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; } - begin_idx = std::min( - begin_idx + traj_param_.num_offset_for_begin_idx, static_cast(path_points.size()) - 1); + + 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++) { @@ -442,8 +294,8 @@ CandidatePoints EBPathOptimizer::getCandidatePoints( 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; + debug_data_ptr->fixed_points = candidate_points.fixed_points; + debug_data_ptr->non_fixed_points = candidate_points.non_fixed_points; return candidate_points; } @@ -451,7 +303,7 @@ std::vector EBPathOptimizer::getPaddedInterpolatedPoi const std::vector & interpolated_points, const int farthest_point_idx) { std::vector padded_interpolated_points; - for (int i = 0; i < traj_param_.num_sampling_points; i++) { + 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 { @@ -461,7 +313,7 @@ std::vector EBPathOptimizer::getPaddedInterpolatedPoi return padded_interpolated_points; } -CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( +EBPathOptimizer::CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( const std::vector & path_points) { double accum_arc_length = 0; @@ -469,12 +321,11 @@ CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( std::vector fixed_points; for (size_t i = 0; i < path_points.size(); i++) { if (i > 0) { - accum_arc_length += - util::calculate2DDistance(path_points[i].pose.position, path_points[i - 1].pose.position); + accum_arc_length += tier4_autoware_utils::calcDistance2d( + path_points[i].pose.position, path_points[i - 1].pose.position); } if ( - accum_arc_length > - traj_param_.num_sampling_points * traj_param_.delta_arc_length_for_optimization) { + accum_arc_length > eb_param_.num_sampling_points_for_eb * eb_param_.delta_arc_length_for_eb) { break; } end_path_idx = i; @@ -487,99 +338,6 @@ CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( return candidate_points; } -bool EBPathOptimizer::isPointInsideDrivableArea( - const geometry_msgs::msg::Point & point, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info) -{ - bool is_inside = true; - unsigned char occupancy_value = std::numeric_limits::max(); - const auto image_point = util::transformMapToOptionalImage(point, map_info); - if (image_point) { - occupancy_value = drivable_area.ptr( - static_cast(image_point.get().y))[static_cast(image_point.get().x)]; - } - if (!image_point || occupancy_value < epsilon_) { - is_inside = false; - } - return is_inside; -} - -int EBPathOptimizer::getEndPathIdx( - const std::vector & path_points, - const int begin_path_idx, const double required_trajectory_length) -{ - double accum_dist = 0; - int end_path_idx = begin_path_idx; - for (std::size_t i = begin_path_idx; i < path_points.size(); i++) { - if (static_cast(i) > begin_path_idx) { - const double dist = - util::calculate2DDistance(path_points[i].pose.position, path_points[i - 1].pose.position); - accum_dist += dist; - } - end_path_idx = i; - if (accum_dist > required_trajectory_length) { - break; - } - } - return end_path_idx; -} - -int EBPathOptimizer::getEndPathIdxInsideArea( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const int begin_path_idx, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info) -{ - int end_path_idx = path_points.size() - 1; - const int default_idx = 0; - const int initial_idx = util::getNearestIdx( - path_points, ego_pose, default_idx, traj_param_.delta_yaw_threshold_for_closest_point); - for (std::size_t i = initial_idx; i < path_points.size(); i++) { - geometry_msgs::msg::Point p = path_points[i].pose.position; - geometry_msgs::msg::Point top_image_point; - end_path_idx = i; - if (util::transformMapToImage(p, map_info, top_image_point)) { - const unsigned char value = drivable_area.ptr( - static_cast(top_image_point.y))[static_cast(top_image_point.x)]; - if (value < epsilon_) { - break; - } - } else { - break; - } - } - - int end_path_idx_inside_drivable_area = begin_path_idx; - for (int i = end_path_idx; i >= begin_path_idx; i--) { - geometry_msgs::msg::Point rel_top_left_point; - rel_top_left_point.x = vehicle_param_.length; - rel_top_left_point.y = vehicle_param_.width * 0.5; - geometry_msgs::msg::Point abs_top_left_point = - util::transformToAbsoluteCoordinate2D(rel_top_left_point, path_points[i].pose); - geometry_msgs::msg::Point top_left_image_point; - - geometry_msgs::msg::Point rel_top_right_point; - rel_top_right_point.x = vehicle_param_.length; - rel_top_right_point.y = -vehicle_param_.width * 0.5; - geometry_msgs::msg::Point abs_top_right_point = - util::transformToAbsoluteCoordinate2D(rel_top_right_point, path_points[i].pose); - geometry_msgs::msg::Point top_right_image_point; - if ( - util::transformMapToImage(abs_top_left_point, map_info, top_left_image_point) && - util::transformMapToImage(abs_top_right_point, map_info, top_right_image_point)) { - const unsigned char top_left_occupancy_value = drivable_area.ptr( - static_cast(top_left_image_point.y))[static_cast(top_left_image_point.x)]; - const unsigned char top_right_occupancy_value = drivable_area.ptr( - static_cast(top_right_image_point.y))[static_cast(top_right_image_point.x)]; - if (top_left_occupancy_value > epsilon_ && top_right_occupancy_value > epsilon_) { - end_path_idx_inside_drivable_area = i; - break; - } - } - } - return end_path_idx_inside_drivable_area; -} - int EBPathOptimizer::getNumFixedPoints( const std::vector & fixed_points, const std::vector & interpolated_points, const int farthest_idx) @@ -587,7 +345,7 @@ int EBPathOptimizer::getNumFixedPoints( int num_fixed_points = 0; if (!fixed_points.empty() && !interpolated_points.empty()) { std::vector interpolated_points = - util::getInterpolatedPoints(fixed_points, traj_param_.delta_arc_length_for_optimization); + 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); @@ -603,16 +361,16 @@ EBPathOptimizer::convertOptimizedPointsToTrajectory( 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 + traj_param_.num_sampling_points]; + tmp_point.pose.position.y = optimized_points[i + eb_param_.num_sampling_points_for_eb]; tmp_point.longitudinal_velocity_mps = constraints[i].velocity; traj_points.push_back(tmp_point); } for (size_t i = 0; i < traj_points.size(); i++) { if (i > 0) { - traj_points[i].pose.orientation = util::getQuaternionFromPoints( + 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 = util::getQuaternionFromPoints( + traj_points[i].pose.orientation = geometry_utils::getQuaternionFromPoints( traj_points[i + 1].pose.position, traj_points[i].pose.position); } } @@ -622,402 +380,84 @@ EBPathOptimizer::convertOptimizedPointsToTrajectory( 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, - [[maybe_unused]] const cv::Mat & clearance_map, const cv::Mat & only_objects_clearance_map) + const int farthest_point_idx, const int straight_idx) { - const nav_msgs::msg::MapMetaData map_info = path.drivable_area.info; - std::vector smooth_constrain_rects(traj_param_.num_sampling_points); - for (int i = 0; i < traj_param_.num_sampling_points; i++) { + 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) { - const auto rect = getConstrainRectangle(anchor, constrain_param_.clearance_for_fixing); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + // 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 - traj_param_.num_joint_buffer_points && - i <= num_fixed_points + traj_param_.num_joint_buffer_points) { - const auto rect = getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + 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, constrain_param_.clearance_for_straight_line); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_straight_line); + smooth_constrain_rects[i] = rect; } else { - const auto rect = - getConstrainRectangle(anchor, constrain_param_.clearance_for_only_smoothing); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + 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; } -boost::optional> EBPathOptimizer::getConstrainRectangleVec( - const bool enable_avoidance, 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, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data) -{ - const nav_msgs::msg::MapMetaData map_info = path.drivable_area.info; - std::vector object_road_constrain_ranges(traj_param_.num_sampling_points); - std::vector road_constrain_ranges(traj_param_.num_sampling_points); - std::vector only_smooth_constrain_ranges(traj_param_.num_sampling_points); - for (int i = 0; i < traj_param_.num_sampling_points; i++) { - const Anchor anchor = getAnchor(interpolated_points, i, path.points); - if (i == 0 || i == 1 || i >= farthest_point_idx - 1 || i < num_fixed_points - 1) { - const ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_fixing); - object_road_constrain_ranges[i] = getUpdatedConstrainRectangle( - rectangle, anchor.pose.position, map_info, only_objects_clearance_map); - road_constrain_ranges[i] = rectangle; - only_smooth_constrain_ranges[i] = rectangle; - } else { - if ( - i >= num_fixed_points - traj_param_.num_joint_buffer_points && - i <= num_fixed_points + traj_param_.num_joint_buffer_points) { - const ConstrainRectangle rectangle = - getConstrainRectangle(path.points, anchor, clearance_map, map_info); - object_road_constrain_ranges[i] = getUpdatedConstrainRectangle( - rectangle, anchor.pose.position, map_info, only_objects_clearance_map); - road_constrain_ranges[i] = rectangle; - only_smooth_constrain_ranges[i] = rectangle; - } else { - if (i >= straight_idx) { - const ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_straight_line); - object_road_constrain_ranges[i] = getUpdatedConstrainRectangle( - rectangle, anchor.pose.position, map_info, only_objects_clearance_map); - road_constrain_ranges[i] = rectangle; - only_smooth_constrain_ranges[i] = rectangle; - } else { - const ConstrainRectangles constrain_rectangles = - getConstrainRectangles(anchor, clearance_map, only_objects_clearance_map, map_info); - object_road_constrain_ranges[i] = constrain_rectangles.object_constrain_rectangle; - road_constrain_ranges[i] = constrain_rectangles.road_constrain_rectangle; - only_smooth_constrain_ranges[i] = - getConstrainRectangle(anchor, constrain_param_.clearance_for_only_smoothing); - } - } - } - } - debug_data->foa_data = - getFOAData(object_road_constrain_ranges, interpolated_points, farthest_point_idx); - boost::optional> constrain_ranges = - getPostProcessedConstrainRectangles( - enable_avoidance, object_road_constrain_ranges, road_constrain_ranges, - only_smooth_constrain_ranges, interpolated_points, path.points, farthest_point_idx, - num_fixed_points, straight_idx, debug_data); - return constrain_ranges; -} - -std::vector EBPathOptimizer::getConstrainRectangleVec( - const std::vector & path_points, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx) -{ - std::vector only_smooth_constrain_ranges(traj_param_.num_sampling_points); - for (int i = 0; i < traj_param_.num_sampling_points; i++) { - const Anchor anchor = getAnchor(interpolated_points, i, path_points); - if (i < num_fixed_points || i >= farthest_point_idx - 1) { - ConstrainRectangle rectangle = getConstrainRectangle(anchor, 0); - only_smooth_constrain_ranges[i] = rectangle; - } else { - if ( - i >= num_fixed_points && - i <= num_fixed_points + traj_param_.num_joint_buffer_points_for_extending) { - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.range_for_extend_joint); - only_smooth_constrain_ranges[i] = rectangle; - } else { - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_only_smoothing); - only_smooth_constrain_ranges[i] = rectangle; - } - } - } - return only_smooth_constrain_ranges; -} - -boost::optional> -EBPathOptimizer::getPostProcessedConstrainRectangles( - const bool enable_avoidance, const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - const int farthest_point_idx, const int num_fixed_points, const int straight_idx, - DebugData * debug_data) const -{ - bool is_using_road_constrain = false; - if (!enable_avoidance) { - is_using_road_constrain = true; - } - bool is_using_only_smooth_constrain = false; - if (isFixingPathPoint(path_points)) { - is_using_only_smooth_constrain = true; - } - if (constrain_param_.is_getting_constraints_close2path_points) { - return getConstrainRectanglesClose2PathPoints( - is_using_only_smooth_constrain, is_using_road_constrain, object_constrains, road_constrains, - only_smooth_constrains, debug_data); - - } else { - return getConstrainRectanglesWithinArea( - is_using_only_smooth_constrain, is_using_road_constrain, farthest_point_idx, num_fixed_points, - straight_idx, object_constrains, road_constrains, only_smooth_constrains, interpolated_points, - path_points, debug_data); - } -} - -boost::optional> -EBPathOptimizer::getConstrainRectanglesClose2PathPoints( - const bool is_using_only_smooth_constrain, const bool is_using_road_constrain, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const -{ - if (is_using_only_smooth_constrain) { - return only_smooth_constrains; - } - if (is_using_road_constrain) { - return getValidConstrainRectangles(road_constrains, only_smooth_constrains, debug_data); - } else { - return getValidConstrainRectangles(object_constrains, only_smooth_constrains, debug_data); - } -} - -boost::optional> EBPathOptimizer::getValidConstrainRectangles( - const std::vector & constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const -{ - bool only_smooth = true; - int debug_cnt = 0; - for (const auto & rect : constrains) { - if (rect.is_empty_driveable_area) { - debug_data->constrain_rectangles = constrains; - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, "Constraint failed at %d", - debug_cnt); - return boost::none; - } - if (!rect.is_including_only_smooth_range) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Constraint does not include only smooth range at %d", debug_cnt); - only_smooth = false; - } - debug_cnt++; - } - if (only_smooth) { - debug_data->constrain_rectangles = only_smooth_constrains; - return only_smooth_constrains; - } else { - debug_data->constrain_rectangles = constrains; - return constrains; - } -} - -boost::optional> EBPathOptimizer::getConstrainRectanglesWithinArea( - bool is_using_only_smooth_constrain, bool is_using_road_constrain, const int farthest_point_idx, - const int num_fixed_points, const int straight_idx, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - DebugData * debug_data) const -{ - if (is_using_road_constrain) { - debug_data->constrain_rectangles = road_constrains; - } else if (is_using_only_smooth_constrain) { - debug_data->constrain_rectangles = only_smooth_constrains; - } else { - debug_data->constrain_rectangles = object_constrains; - } - - std::vector constrain_ranges(traj_param_.num_sampling_points); - int origin_dynamic_joint_idx = traj_param_.num_sampling_points; - for (int i = 0; i < traj_param_.num_sampling_points; i++) { - if (isPreFixIdx(i, farthest_point_idx, num_fixed_points, straight_idx)) { - constrain_ranges[i] = only_smooth_constrains[i]; - if (object_constrains[i].is_empty_driveable_area) { - is_using_road_constrain = true; - return boost::none; - } - } else { - if ( - i > origin_dynamic_joint_idx && - i <= origin_dynamic_joint_idx + traj_param_.num_joint_buffer_points) { - const Anchor anchor = getAnchor(interpolated_points, i, path_points); - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - constrain_ranges[i] = rectangle; - } else if (is_using_only_smooth_constrain) { - constrain_ranges[i] = only_smooth_constrains[i]; - } else if (is_using_road_constrain) { - constrain_ranges[i] = road_constrains[i]; - if (constrain_ranges[i].is_empty_driveable_area) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Only road clearance optimization failed at %d", i); - is_using_only_smooth_constrain = true; - origin_dynamic_joint_idx = i; - return boost::none; - } - } else { - constrain_ranges[i] = object_constrains[i]; - if (constrain_ranges[i].is_empty_driveable_area) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Object clearance optimization failed at %d", i); - return boost::none; - } - } - } - } - return constrain_ranges; -} - -bool EBPathOptimizer::isPreFixIdx( - const int target_idx, const int farthest_point_idx, const int num_fixed_points, - const int straight_idx) const -{ - if ( - target_idx == 0 || target_idx == 1 || target_idx >= farthest_point_idx - 1 || - target_idx < num_fixed_points - 1 || - (target_idx >= num_fixed_points - traj_param_.num_joint_buffer_points && - target_idx <= num_fixed_points + traj_param_.num_joint_buffer_points) || - target_idx >= straight_idx) { - return true; - } else { - return false; - } -} - -bool EBPathOptimizer::isClose2Object( - const geometry_msgs::msg::Point & point, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map, const double distance_threshold) const -{ - const auto image_point = util::transformMapToOptionalImage(point, map_info); - if (!image_point) { - return false; - } - const float object_clearance = only_objects_clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - if (object_clearance < distance_threshold) { - return true; - } - return false; -} - void EBPathOptimizer::updateConstrain( const std::vector & interpolated_points, - const std::vector & rectangle_points, const OptMode & opt_mode) + 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(traj_param_.num_sampling_points * 2, 0.0); - std::vector upper_bound(traj_param_.num_sampling_points * 2, 0.0); - for (int i = 0; i < traj_param_.num_sampling_points; ++i) { + 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 + traj_param_.num_sampling_points) = constrain.top_and_bottom.y_coef; - A(i + traj_param_.num_sampling_points, i) = constrain.left_and_right.x_coef; - A(i + traj_param_.num_sampling_points, i + traj_param_.num_sampling_points) = - constrain.left_and_right.y_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 + traj_param_.num_sampling_points] = constrain.left_and_right.lower_bound; - upper_bound[i + traj_param_.num_sampling_points] = constrain.left_and_right.upper_bound; - } - if (opt_mode == OptMode::Normal) { - osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - osqp_solver_ptr_->updateA(A); - } else if (opt_mode == OptMode::Extending) { - ex_osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - ex_osqp_solver_ptr_->updateA(A); - } else if (opt_mode == OptMode::Visualizing) { - vis_osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - vis_osqp_solver_ptr_->updateA(A); + lower_bound[i + num_points] = constrain.left_and_right.lower_bound; + upper_bound[i + num_points] = constrain.left_and_right.upper_bound; } -} - -Rectangle EBPathOptimizer::getAbsShapeRectangle( - const Rectangle & rel_shape_rectangle_points, const geometry_msgs::msg::Point & offset_point, - const geometry_msgs::msg::Pose & origin) const -{ - geometry_msgs::msg::Point abs_target_point = - util::transformToAbsoluteCoordinate2D(offset_point, origin); - - geometry_msgs::msg::Point abs_top_left; - abs_top_left.x = (rel_shape_rectangle_points.top_left.x + abs_target_point.x); - abs_top_left.y = (rel_shape_rectangle_points.top_left.y + abs_target_point.y); - - geometry_msgs::msg::Point abs_top_right; - abs_top_right.x = (rel_shape_rectangle_points.top_right.x + abs_target_point.x); - abs_top_right.y = (rel_shape_rectangle_points.top_right.y + abs_target_point.y); - - geometry_msgs::msg::Point abs_bottom_left; - abs_bottom_left.x = (rel_shape_rectangle_points.bottom_left.x + abs_target_point.x); - abs_bottom_left.y = (rel_shape_rectangle_points.bottom_left.y + abs_target_point.y); - - geometry_msgs::msg::Point abs_bottom_right; - abs_bottom_right.x = (rel_shape_rectangle_points.bottom_right.x + abs_target_point.x); - abs_bottom_right.y = (rel_shape_rectangle_points.bottom_right.y + abs_target_point.y); - Rectangle abs_shape_rectangle_points; - abs_shape_rectangle_points.top_left = abs_top_left; - abs_shape_rectangle_points.top_right = abs_top_right; - abs_shape_rectangle_points.bottom_left = abs_bottom_left; - abs_shape_rectangle_points.bottom_right = abs_bottom_right; - return abs_shape_rectangle_points; + osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); + osqp_solver_ptr_->updateA(A); } -geometry_msgs::msg::Pose EBPathOptimizer::getOriginPose( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points) -{ - geometry_msgs::msg::Pose pose; - pose.position = interpolated_points[interpolated_idx]; - if (interpolated_idx > 0) { - pose.orientation = util::getQuaternionFromPoints( - interpolated_points[interpolated_idx], interpolated_points[interpolated_idx - 1]); - } else if (interpolated_idx == 0 && interpolated_points.size() > 1) { - pose.orientation = util::getQuaternionFromPoints( - interpolated_points[interpolated_idx + 1], interpolated_points[interpolated_idx]); - } - const int default_idx = 0; - const int nearest_id = util::getNearestIdx( - path_points, pose, default_idx, traj_param_.delta_yaw_threshold_for_closest_point); - const geometry_msgs::msg::Quaternion nearest_q = path_points[nearest_id].pose.orientation; - geometry_msgs::msg::Pose origin; - origin.position = interpolated_points[interpolated_idx]; - origin.orientation = nearest_q; - return origin; -} - -Anchor EBPathOptimizer::getAnchor( +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 = util::getQuaternionFromPoints( + 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 = util::getQuaternionFromPoints( + pose.orientation = geometry_utils::getQuaternionFromPoints( interpolated_points[interpolated_idx + 1], interpolated_points[interpolated_idx]); } - const int default_idx = 0; - const int nearest_idx = util::getNearestIdx( - path_points, pose, default_idx, traj_param_.delta_yaw_threshold_for_closest_point); + const auto opt_nearest_idx = tier4_autoware_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]; @@ -1026,268 +466,26 @@ Anchor EBPathOptimizer::getAnchor( return anchor; } -boost::optional>> -EBPathOptimizer::getOccupancyPoints( - const geometry_msgs::msg::Pose & origin, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - geometry_msgs::msg::Point image_point; - if (!util::transformMapToImage(origin.position, map_info, image_point)) { - return boost::none; - } - const float clearance = std::max( - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)] * - map_info.resolution, - static_cast(keep_space_shape_ptr_->y)); - const float y_constrain_search_range = clearance - 0.5 * keep_space_shape_ptr_->y; - int y_side_length = 0; - for (float y = -y_constrain_search_range; y <= y_constrain_search_range + epsilon_; - y += map_info.resolution * constrain_param_.coef_y_constrain_search_resolution) { - y_side_length++; - } - const float x_constrain_search_range = - std::fmin(constrain_param_.max_x_constrain_search_range, y_constrain_search_range); - int x_side_length = 0; - for (float x = -1 * x_constrain_search_range; x <= x_constrain_search_range + epsilon_; - x += map_info.resolution * constrain_param_.coef_x_constrain_search_resolution) { - x_side_length++; - } - if (x_side_length == 0 || y_side_length == 0) { - return boost::none; - } - std::vector> occupancy_points( - x_side_length, std::vector(y_side_length)); - int x_idx_in_occupancy_map = 0; - int y_idx_in_occupancy_map = 0; - for (float x = -1 * x_constrain_search_range; x <= x_constrain_search_range + epsilon_; - x += map_info.resolution * constrain_param_.coef_x_constrain_search_resolution) { - for (float y = -1 * y_constrain_search_range; y <= y_constrain_search_range + epsilon_; - y += map_info.resolution * constrain_param_.coef_y_constrain_search_resolution) { - geometry_msgs::msg::Point relative_point; - relative_point.x = x; - relative_point.y = y; - geometry_msgs::msg::Point abs_point = - util::transformToAbsoluteCoordinate2D(relative_point, origin); - const int x_idx = x_side_length - x_idx_in_occupancy_map - 1; - const int y_idx = y_side_length - y_idx_in_occupancy_map - 1; - if (x_idx < 0 || x_idx >= x_side_length || y_idx < 0 || y_idx >= y_side_length) { - continue; - } - occupancy_points[x_idx][y_idx] = abs_point; - y_idx_in_occupancy_map++; - } - x_idx_in_occupancy_map++; - y_idx_in_occupancy_map = 0; - } - return occupancy_points; -} - -Rectangle EBPathOptimizer::getRelShapeRectangle( - const geometry_msgs::msg::Vector3 & vehicle_shape, const geometry_msgs::msg::Pose & origin) const -{ - geometry_msgs::msg::Point top_left; - top_left.x = vehicle_shape.x; - top_left.y = 0.5 * vehicle_shape.y; - geometry_msgs::msg::Point top_right; - top_right.x = vehicle_shape.x; - top_right.y = -0.5 * vehicle_shape.y; - geometry_msgs::msg::Point bottom_left; - bottom_left.x = 0.0; - bottom_left.y = 0.5 * vehicle_shape.y; - geometry_msgs::msg::Point bottom_right; - bottom_right.x = 0.0; - bottom_right.y = -0.5 * vehicle_shape.y; - - geometry_msgs::msg::Pose tmp_origin; - tmp_origin.orientation = origin.orientation; - top_left = util::transformToAbsoluteCoordinate2D(top_left, tmp_origin); - top_right = util::transformToAbsoluteCoordinate2D(top_right, tmp_origin); - bottom_left = util::transformToAbsoluteCoordinate2D(bottom_left, tmp_origin); - bottom_right = util::transformToAbsoluteCoordinate2D(bottom_right, tmp_origin); - Rectangle rectangle; - rectangle.top_left = top_left; - rectangle.top_right = top_right; - rectangle.bottom_left = bottom_left; - rectangle.bottom_right = bottom_right; - return rectangle; -} - -ConstrainRectangles EBPathOptimizer::getConstrainRectangles( - const Anchor & anchor, const cv::Mat & clearance_map, const cv::Mat & only_objects_clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const auto occupancy_points_opt = getOccupancyPoints(anchor.pose, clearance_map, map_info); - const auto image_point = util::transformMapToOptionalImage(anchor.pose.position, map_info); - if (!image_point || !occupancy_points_opt) { - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - rectangle.is_empty_driveable_area = true; - ConstrainRectangles constrain_rectangles; - constrain_rectangles.object_constrain_rectangle = rectangle; - constrain_rectangles.road_constrain_rectangle = rectangle; - return constrain_rectangles; - } - OccupancyMaps occupancy_maps = getOccupancyMaps( - occupancy_points_opt.get(), anchor.pose, image_point.get(), clearance_map, - only_objects_clearance_map, map_info); - - ConstrainRectangle object_constrain = getConstrainRectangle( - occupancy_maps.object_occupancy_map, occupancy_points_opt.get(), anchor, map_info, - only_objects_clearance_map); - - ConstrainRectangles constrain_rectangles; - constrain_rectangles.object_constrain_rectangle = getUpdatedConstrainRectangle( - object_constrain, anchor.pose.position, map_info, only_objects_clearance_map); - constrain_rectangles.road_constrain_rectangle = getConstrainRectangle( - occupancy_maps.road_occupancy_map, occupancy_points_opt.get(), anchor, map_info, - only_objects_clearance_map); - return constrain_rectangles; -} - -OccupancyMaps EBPathOptimizer::getOccupancyMaps( - const std::vector> & occupancy_points, - const geometry_msgs::msg::Pose & origin_pose, - const geometry_msgs::msg::Point & origin_point_in_image, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info) const -{ - Rectangle rel_shape_rectangles = getRelShapeRectangle(*keep_space_shape_ptr_, origin_pose); - const float clearance = std::max( - clearance_map.ptr( - static_cast(origin_point_in_image.y))[static_cast(origin_point_in_image.x)] * - map_info.resolution, - static_cast(keep_space_shape_ptr_->y)); - const float y_constrain_search_range = clearance - 0.5 * keep_space_shape_ptr_->y; - const float x_constrain_search_range = - std::fmin(constrain_param_.max_x_constrain_search_range, y_constrain_search_range); - std::vector> object_occupancy_map( - occupancy_points.size(), std::vector(occupancy_points.front().size(), 0)); - std::vector> road_occupancy_map( - occupancy_points.size(), std::vector(occupancy_points.front().size(), 0)); - int x_idx_in_occupancy_map = 0; - int y_idx_in_occupancy_map = 0; - for (float x = -1 * x_constrain_search_range; x <= x_constrain_search_range + epsilon_; - x += map_info.resolution * constrain_param_.coef_x_constrain_search_resolution) { - for (float y = -1 * y_constrain_search_range; y <= y_constrain_search_range + epsilon_; - y += map_info.resolution * constrain_param_.coef_y_constrain_search_resolution) { - geometry_msgs::msg::Point rel_target_point; - rel_target_point.x = x; - rel_target_point.y = y; - Rectangle abs_shape_rectangles = - getAbsShapeRectangle(rel_shape_rectangles, rel_target_point, origin_pose); - float top_left_clearance = std::numeric_limits::lowest(); - float top_left_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point top_left_image; - if (util::transformMapToImage(abs_shape_rectangles.top_left, map_info, top_left_image)) { - top_left_clearance = clearance_map.ptr(static_cast( - top_left_image.y))[static_cast(top_left_image.x)] * - map_info.resolution; - top_left_objects_clearance = only_objects_clearance_map.ptr(static_cast( - top_left_image.y))[static_cast(top_left_image.x)] * - map_info.resolution; - } - - float top_right_clearance = std::numeric_limits::lowest(); - float top_right_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point top_right_image; - if (util::transformMapToImage(abs_shape_rectangles.top_right, map_info, top_right_image)) { - top_right_clearance = clearance_map.ptr(static_cast( - top_right_image.y))[static_cast(top_right_image.x)] * - map_info.resolution; - top_right_objects_clearance = only_objects_clearance_map.ptr(static_cast( - top_right_image.y))[static_cast(top_right_image.x)] * - map_info.resolution; - } - float bottom_left_clearance = std::numeric_limits::lowest(); - float bottom_left_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point bottom_left_image; - if (util::transformMapToImage( - abs_shape_rectangles.bottom_left, map_info, bottom_left_image)) { - bottom_left_clearance = clearance_map.ptr(static_cast( - bottom_left_image.y))[static_cast(bottom_left_image.x)] * - map_info.resolution; - bottom_left_objects_clearance = - only_objects_clearance_map.ptr( - static_cast(bottom_left_image.y))[static_cast(bottom_left_image.x)] * - map_info.resolution; - } - float bottom_right_clearance = std::numeric_limits::lowest(); - float bottom_right_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point bottom_right_image; - if (util::transformMapToImage( - abs_shape_rectangles.bottom_right, map_info, bottom_right_image)) { - bottom_right_clearance = clearance_map.ptr(static_cast( - bottom_right_image.y))[static_cast(bottom_right_image.x)] * - map_info.resolution; - bottom_right_objects_clearance = - only_objects_clearance_map.ptr( - static_cast(bottom_right_image.y))[static_cast(bottom_right_image.x)] * - map_info.resolution; - } - - const int x_idx = occupancy_points.size() - x_idx_in_occupancy_map - 1; - const int y_idx = occupancy_points.front().size() - y_idx_in_occupancy_map - 1; - if ( - x_idx < 0 || x_idx >= static_cast(occupancy_points.size()) || y_idx < 0 || - y_idx >= static_cast(occupancy_points.front().size())) { - continue; - } - if ( - top_left_clearance < constrain_param_.clearance_from_road || - top_right_clearance < constrain_param_.clearance_from_road || - bottom_right_clearance < constrain_param_.clearance_from_road || - bottom_left_clearance < constrain_param_.clearance_from_road || - top_left_objects_clearance < constrain_param_.clearance_from_object || - top_right_objects_clearance < constrain_param_.clearance_from_object || - bottom_right_objects_clearance < constrain_param_.clearance_from_object || - bottom_left_objects_clearance < constrain_param_.clearance_from_object) { - object_occupancy_map[x_idx][y_idx] = 1; - } - if ( - top_left_clearance < constrain_param_.clearance_from_road || - top_right_clearance < constrain_param_.clearance_from_road || - bottom_right_clearance < constrain_param_.clearance_from_road || - bottom_left_clearance < constrain_param_.clearance_from_road) { - road_occupancy_map[x_idx][y_idx] = 1; - } - y_idx_in_occupancy_map++; - } - x_idx_in_occupancy_map++; - y_idx_in_occupancy_map = 0; - } - OccupancyMaps occupancy_maps; - occupancy_maps.object_occupancy_map = object_occupancy_map; - occupancy_maps.road_occupancy_map = road_occupancy_map; - return occupancy_maps; -} - int EBPathOptimizer::getStraightLineIdx( const std::vector & interpolated_points, const int farthest_point_idx, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info, 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 = util::getYawFromPoints(interpolated_points[i + 1], interpolated_points[i]); + 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 = util::normalizeRadian(delta_yaw); - float clearance_from_object = std::numeric_limits::max(); - const auto image_point = util::transformMapToOptionalImage(interpolated_points[i], map_info); - if (image_point) { - clearance_from_object = only_objects_clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - } - if ( - std::fabs(norm_delta_yaw) > traj_param_.delta_yaw_threshold_for_straight || - clearance_from_object < constrain_param_.clearance_from_object_for_straight) { + 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 = util::getYawFromPoints(interpolated_points[i], interpolated_points[i - 1]); + const double yaw = + tier4_autoware_utils::calcAzimuthAngle(interpolated_points[i - 1], interpolated_points[i]); prev_yaw = yaw; } } @@ -1297,7 +495,7 @@ int EBPathOptimizer::getStraightLineIdx( return straight_line_idx; } -Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( +EBPathOptimizer::Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( const geometry_msgs::msg::Point & interpolated_point, const ConstrainRectangle & constrain_range) { Constrain constrain; @@ -1353,7 +551,7 @@ Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( return constrain; } -ConstrainLines EBPathOptimizer::getConstrainLines( +EBPathOptimizer::ConstrainLines EBPathOptimizer::getConstrainLines( const double dx, const double dy, const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & opposite_point) { @@ -1381,213 +579,35 @@ ConstrainRectangle EBPathOptimizer::getConstrainRectangle( geometry_msgs::msg::Point top_left; top_left.x = clearance; top_left.y = clearance; - constrain_range.top_left = util::transformToAbsoluteCoordinate2D(top_left, anchor.pose); + 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 = util::transformToAbsoluteCoordinate2D(top_right, anchor.pose); + 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 = util::transformToAbsoluteCoordinate2D(bottom_left, anchor.pose); + 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 = util::transformToAbsoluteCoordinate2D(bottom_right, anchor.pose); + constrain_range.bottom_right = + geometry_utils::transformToAbsoluteCoordinate2D(bottom_right, anchor.pose); constrain_range.velocity = anchor.velocity; return constrain_range; } -ConstrainRectangle EBPathOptimizer::getUpdatedConstrainRectangle( - const ConstrainRectangle & rectangle, const geometry_msgs::msg::Point & candidate_point, - const nav_msgs::msg::MapMetaData & map_info, const cv::Mat & only_objects_clearance_map) const -{ - auto rect = rectangle; - if (isClose2Object( - candidate_point, map_info, only_objects_clearance_map, - constrain_param_.min_object_clearance_for_deceleration)) { - rect.velocity = std::fmin(rect.velocity, traj_param_.max_avoiding_ego_velocity_ms); - } - if (isClose2Object( - candidate_point, map_info, only_objects_clearance_map, - constrain_param_.min_object_clearance_for_joint)) { - rect.is_including_only_smooth_range = false; - } - return rect; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const std::vector & path_points, - const Anchor & anchor, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const int default_idx = -1; - const auto interpolated_path_points = - util::getInterpolatedPoints(path_points, traj_param_.delta_arc_length_for_trajectory); - const int nearest_idx = util::getNearestIdx( - interpolated_path_points, anchor.pose, default_idx, - traj_param_.delta_yaw_threshold_for_closest_point); - - float clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point image_point; - if (util::transformMapToImage(interpolated_path_points[nearest_idx], map_info, image_point)) { - clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)] * - map_info.resolution; - } - const double dist = - util::calculate2DDistance(anchor.pose.position, interpolated_path_points[nearest_idx]); - ConstrainRectangle constrain_rectangle; - // idx is valid && anchor is close to nearest path point - if (nearest_idx > default_idx && dist < clearance) { - constrain_rectangle = - getConstrainRectangle(anchor, nearest_idx, interpolated_path_points, clearance_map, map_info); - } else { - constrain_rectangle = getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - } - return constrain_rectangle; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const Anchor & anchor, const int & nearest_idx, - const std::vector & interpolated_points, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - Anchor replaced_anchor = anchor; - replaced_anchor.pose.position = interpolated_points[nearest_idx]; - if (nearest_idx > 0) { - replaced_anchor.pose.orientation = util::getQuaternionFromPoints( - interpolated_points[nearest_idx], interpolated_points[nearest_idx - 1]); - } else if (nearest_idx == 0 && interpolated_points.size() > 1) { - replaced_anchor.pose.orientation = util::getQuaternionFromPoints( - interpolated_points[nearest_idx + 1], interpolated_points[nearest_idx]); - } - ConstrainRectangles rectangles = - getConstrainRectangles(replaced_anchor, clearance_map, clearance_map, map_info); - const double rel_plus_y = util::calculate2DDistance( - rectangles.road_constrain_rectangle.top_left, replaced_anchor.pose.position); - const double rel_minus_y = util::calculate2DDistance( - rectangles.road_constrain_rectangle.top_right, replaced_anchor.pose.position); - - ConstrainRectangle constrain_rectangle; - geometry_msgs::msg::Point top_left; - top_left.x = constrain_param_.clearance_for_joint; - top_left.y = rel_plus_y; - constrain_rectangle.top_left = - util::transformToAbsoluteCoordinate2D(top_left, replaced_anchor.pose); - geometry_msgs::msg::Point top_right; - top_right.x = constrain_param_.clearance_for_joint; - top_right.y = -1 * rel_minus_y; - constrain_rectangle.top_right = - util::transformToAbsoluteCoordinate2D(top_right, replaced_anchor.pose); - geometry_msgs::msg::Point bottom_left; - bottom_left.x = -1 * constrain_param_.clearance_for_joint; - bottom_left.y = rel_plus_y; - constrain_rectangle.bottom_left = - util::transformToAbsoluteCoordinate2D(bottom_left, replaced_anchor.pose); - geometry_msgs::msg::Point bottom_right; - bottom_right.x = -1 * constrain_param_.clearance_for_joint; - bottom_right.y = -1 * rel_minus_y; - constrain_rectangle.bottom_right = - util::transformToAbsoluteCoordinate2D(bottom_right, replaced_anchor.pose); - constrain_rectangle.velocity = anchor.velocity; - return constrain_rectangle; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const std::vector> & occupancy_points, - const util::Rectangle & util_rect, const Anchor & anchor) const -{ - ConstrainRectangle constrain_rectangle; - constrain_rectangle.bottom_left = occupancy_points[util_rect.min_x_idx][util_rect.max_y_idx]; - constrain_rectangle.bottom_right = occupancy_points[util_rect.min_x_idx][util_rect.min_y_idx]; - constrain_rectangle.top_left = occupancy_points[util_rect.max_x_idx][util_rect.max_y_idx]; - constrain_rectangle.top_right = occupancy_points[util_rect.max_x_idx][util_rect.min_y_idx]; - - geometry_msgs::msg::Pose left_pose = anchor.pose; - left_pose.position = constrain_rectangle.top_left; - geometry_msgs::msg::Point top_left; - top_left.x = - std::fmin(keep_space_shape_ptr_->x, constrain_param_.max_lon_space_for_driveable_constraint); - top_left.y = 0; - constrain_rectangle.top_left = util::transformToAbsoluteCoordinate2D(top_left, left_pose); - geometry_msgs::msg::Point bottom_left; - bottom_left.x = 0; - bottom_left.y = 0; - constrain_rectangle.bottom_left = util::transformToAbsoluteCoordinate2D(bottom_left, left_pose); - geometry_msgs::msg::Pose right_pose = anchor.pose; - right_pose.position = constrain_rectangle.top_right; - geometry_msgs::msg::Point top_right; - top_right.x = - std::fmin(keep_space_shape_ptr_->x, constrain_param_.max_lon_space_for_driveable_constraint); - top_right.y = 0; - constrain_rectangle.top_right = util::transformToAbsoluteCoordinate2D(top_right, right_pose); - geometry_msgs::msg::Point bottom_right; - bottom_right.x = 0; - bottom_right.y = 0; - constrain_rectangle.bottom_right = - util::transformToAbsoluteCoordinate2D(bottom_right, right_pose); - constrain_rectangle.velocity = anchor.velocity; - return constrain_rectangle; -} - ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const std::vector> & occupancy_map, - const std::vector> & occupancy_points, - const Anchor & anchor, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map) const -{ - util::Rectangle util_rect = util::getLargestRectangle(occupancy_map); - - ConstrainRectangle constrain_rectangle; - if (util_rect.area < epsilon_) { - constrain_rectangle = getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - constrain_rectangle.is_empty_driveable_area = true; - } else { - constrain_rectangle = getConstrainRectangle(occupancy_points, util_rect, anchor); - } - geometry_msgs::msg::Point max_abs_y = occupancy_points[util_rect.max_x_idx][util_rect.max_y_idx]; - geometry_msgs::msg::Point min_abs_y = occupancy_points[util_rect.max_x_idx][util_rect.min_y_idx]; - geometry_msgs::msg::Point max_rel_y = - util::transformToRelativeCoordinate2D(max_abs_y, anchor.pose); - geometry_msgs::msg::Point min_rel_y = - util::transformToRelativeCoordinate2D(min_abs_y, anchor.pose); - if ( - (max_rel_y.y < -1 * constrain_param_.clearance_for_only_smoothing || - min_rel_y.y > constrain_param_.clearance_for_only_smoothing) && - isClose2Object( - anchor.pose.position, map_info, only_objects_clearance_map, - constrain_param_.clearance_from_object)) { - constrain_rectangle.is_including_only_smooth_range = false; - } - return constrain_rectangle; -} - -bool EBPathOptimizer::isFixingPathPoint( - [[maybe_unused]] const std::vector & path_points) - const -{ - return false; -} - -FOAData EBPathOptimizer::getFOAData( - const std::vector & rectangles, - const std::vector & interpolated_points, const int farthest_idx) -{ - FOAData foa_data; - for (const auto & rect : rectangles) { - if (rect.is_empty_driveable_area) { - foa_data.is_avoidance_possible = false; - } - } - if (!foa_data.is_avoidance_possible) { - RCLCPP_WARN( - rclcpp::get_logger("EBPathOptimizer"), - "[ObstacleAvoidancePlanner] Fail to make new trajectory from empty drivable area"); - } - - foa_data.constrain_rectangles = rectangles; - foa_data.avoiding_traj_points = calculateTrajectory( - interpolated_points, foa_data.constrain_rectangles, farthest_idx, OptMode::Visualizing); - return foa_data; + 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; + rect.velocity = anchor.velocity; + return rect; } diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 28620c3ce5f51..4427cbaf2e437 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -14,879 +14,875 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "obstacle_avoidance_planner/utils.hpp" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "tf2/utils.h" -#include -#include +#include "nav_msgs/msg/map_meta_data.hpp" -#include - -#include - -#include +#include "boost/optional.hpp" #include #include #include #include +#include #include +namespace +{ +geometry_msgs::msg::Pose convertRefPointsToPose(const ReferencePoint & ref_point) +{ + geometry_msgs::msg::Pose pose; + pose.position = ref_point.p; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); + return pose; +} + +std::tuple extractBounds( + const std::vector & ref_points, const size_t l_idx) +{ + 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; + lb_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).lower_bound; + } + return {ub_vec, lb_vec}; +} + +Bounds findWidestBounds(const BoundsCandidates & front_bounds_candidates) +{ + 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; + } + } + } + return front_bounds_candidates.at(max_width_index); +} + +double calcOverlappedBounds( + const geometry_msgs::msg::Pose & front_point, const Bounds & front_bounds_candidate, + const geometry_msgs::msg::Pose & prev_front_point, const Bounds & prev_front_continuous_bounds) +{ + const double avoiding_yaw = + tier4_autoware_utils::normalizeRadian(tf2::getYaw(front_point.orientation) + M_PI_2); + + geometry_msgs::msg::Point ub_pos; + ub_pos.x = front_point.position.x + front_bounds_candidate.upper_bound * std::cos(avoiding_yaw); + ub_pos.y = front_point.position.y + front_bounds_candidate.upper_bound * std::sin(avoiding_yaw); + + geometry_msgs::msg::Point lb_pos; + lb_pos.x = front_point.position.x + front_bounds_candidate.lower_bound * std::cos(avoiding_yaw); + lb_pos.y = front_point.position.y + front_bounds_candidate.lower_bound * std::sin(avoiding_yaw); + + const double projected_ub_y = + geometry_utils::transformToRelativeCoordinate2D(ub_pos, prev_front_point).y; + const double projected_lb_y = + geometry_utils::transformToRelativeCoordinate2D(lb_pos, prev_front_point).y; + + const double min_ub = std::min(projected_ub_y, prev_front_continuous_bounds.upper_bound); + const double max_lb = std::max(projected_lb_y, prev_front_continuous_bounds.lower_bound); + + const double overlapped_length = min_ub - max_lb; + return overlapped_length; +} + +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; +} + +template +void trimPoints(std::vector & points) +{ + 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; + } + } +} + +std::vector eigenVectorToStdVector(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) +{ + 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; +} + +Eigen::Vector2d getState( + const geometry_msgs::msg::Pose & target_pose, const geometry_msgs::msg::Pose & ref_pose) +{ + 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; +} + +std::vector slerpYawFromReferencePoints(const std::vector & ref_points) +{ + if (ref_points.size() == 1) { + return {ref_points.front().yaw}; + } + + std::vector points; + for (const auto & ref_point : ref_points) { + points.push_back(ref_point.p); + } + return interpolation::slerpYawFromPoints(points); +} +} // namespace + MPTOptimizer::MPTOptimizer( - const bool is_showing_debug_info, const QPParam & qp_param, const TrajectoryParam & traj_param, - const ConstrainParam & constraint_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param) -: is_showing_debug_info_(is_showing_debug_info) + 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), + 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_)) { - qp_param_ptr_ = std::make_unique(qp_param); - traj_param_ptr_ = std::make_unique(traj_param); - constraint_param_ptr_ = std::make_unique(constraint_param); - vehicle_param_ptr_ = std::make_unique(vehicle_param); - mpt_param_ptr_ = std::make_unique(mpt_param); - - vehicle_model_ptr_ = std::make_unique( - vehicle_param_ptr_->wheelbase, vehicle_param_ptr_->max_steer_rad, - vehicle_param_ptr_->steer_tau); } -boost::optional MPTOptimizer::getModelPredictiveTrajectory( +boost::optional MPTOptimizer::getModelPredictiveTrajectory( const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, const std::unique_ptr & prev_trajs, const CVMaps & maps, - const geometry_msgs::msg::Pose & ego_pose, DebugData * debug_data) + const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, + std::shared_ptr debug_data_ptr) { - auto t_start1 = std::chrono::high_resolution_clock::now(); + 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("MPTOptimizer"), is_showing_debug_info_, + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "return boost::none since smoothed_points is empty"); return boost::none; } - geometry_msgs::msg::Pose origin_pose = smoothed_points.front().pose; - if (prev_trajs) { - const int prev_nearest_idx = util::getNearestIdx( - prev_trajs->model_predictive_trajectory, smoothed_points.front().pose.position); - origin_pose = prev_trajs->model_predictive_trajectory.at(prev_nearest_idx).pose; - } - std::vector ref_points = getReferencePoints( - origin_pose, ego_pose, smoothed_points, path_points, prev_trajs, maps, debug_data); - if (ref_points.empty()) { + std::vector full_ref_points = + getReferencePoints(smoothed_points, prev_trajs, enable_avoidance, maps, debug_data_ptr); + if (full_ref_points.empty()) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "return boost::none since ref_points is empty"); return boost::none; - } - - const auto mpt_matrix = generateMPTMatrix(ref_points, path_points, prev_trajs); - if (!mpt_matrix) { + } else if (full_ref_points.size() == 1) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, - "return boost::none since matrix has nan"); + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, + "return boost::none since ref_points.size() == 1"); return boost::none; } + debug_data_ptr->mpt_fixed_traj = getMPTFixedPoints(full_ref_points); + + 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; + } + + 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)); + } + } + + // calculate B and W matrices + const auto mpt_matrix = generateMPTMatrix(non_fixed_ref_points, debug_data_ptr); + + // calculate Q and R matrices + const auto val_matrix = generateValueMatrix(non_fixed_ref_points, path_points, debug_data_ptr); + const auto optimized_control_variables = executeOptimization( - enable_avoidance, mpt_matrix.get(), ref_points, path_points, maps, debug_data); + prev_trajs, enable_avoidance, mpt_matrix, val_matrix, non_fixed_ref_points, debug_data_ptr); if (!optimized_control_variables) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "return boost::none since could not solve qp"); return boost::none; } - const auto mpt_points = - getMPTPoints(ref_points, optimized_control_variables.get(), mpt_matrix.get(), smoothed_points); + const auto mpt_points = getMPTPoints( + fixed_ref_points, non_fixed_ref_points, optimized_control_variables.get(), mpt_matrix, + debug_data_ptr); + + 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()); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, "MPT time: = %f [ms]", elapsed_ms1); MPTTrajs mpt_trajs; mpt_trajs.mpt = mpt_points; - mpt_trajs.ref_points = ref_points; + mpt_trajs.ref_points = full_optimized_ref_points; return mpt_trajs; } std::vector MPTOptimizer::getReferencePoints( - const geometry_msgs::msg::Pose & origin_pose, const geometry_msgs::msg::Pose & ego_pose, - const std::vector & points, - const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, - DebugData * debug_data) const + const std::vector & smoothed_points, + const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const { - const auto ref_points = - convertToReferencePoints(points, path_points, prev_trajs, ego_pose, maps, debug_data); - - const int begin_idx = util::getNearestPointIdx(ref_points, origin_pose.position); - const auto first_it = ref_points.begin() + begin_idx; - int num_points = std::min( - static_cast(ref_points.size()) - 1 - begin_idx, traj_param_ptr_->num_sampling_points); - num_points = std::max(num_points, 0); - std::vector truncated_points(first_it, first_it + num_points); - calcInitialState(&truncated_points, origin_pose); - return truncated_points; -} + 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 cropped_interpolated_points = points_utils::clipBackwardPoints( + interpolated_points, current_ego_pose_.position, traj_param_.backward_fixing_distance, + mpt_param_.delta_arc_length_for_mpt_points); + + return points_utils::convertToReferencePoints(cropped_interpolated_points); + } -std::vector MPTOptimizer::convertToReferencePoints( - const std::vector & points, - [[maybe_unused]] const std::vector & path_points, - const std::unique_ptr & prev_trajs, - [[maybe_unused]] const geometry_msgs::msg::Pose & ego_pose, [[maybe_unused]] const CVMaps & maps, - DebugData * debug_data) const -{ - const auto interpolated_points = - util::getInterpolatedPoints(points, traj_param_ptr_->delta_arc_length_for_mpt_points); - if (interpolated_points.empty()) { + // calc non fixed traj points + const size_t seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(smoothed_points, fixed_ref_points.back().p); + const auto non_fixed_traj_points = + std::vector{ + smoothed_points.begin() + seg_idx, smoothed_points.end()}; + + const double offset = tier4_autoware_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; + }(); + + if (ref_points.empty()) { + return std::vector{}; + } + + // set some information to reference points considering fix kinematics + trimPoints(ref_points); + calcOrientation(ref_points); + calcVelocity(ref_points, smoothed_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); + + // set bounds information + calcBounds(ref_points, enable_avoidance, maps, debug_data_ptr); + calcVehicleBounds(ref_points, maps, debug_data_ptr, enable_avoidance); + + // set extra information (alpha and has_object_collision) + // NOTE: This must be after bounds calculation. + calcExtraPoints(ref_points, prev_trajs); + + 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); + + // bounds information is assigned to debug data after truncating reference points + debug_data_ptr->ref_points = ref_points; + + return ref_points; + }(); + if (ref_points.empty()) { return std::vector{}; } - auto reference_points = getBaseReferencePoints(interpolated_points, prev_trajs, debug_data); - - calcOrientation(&reference_points); - calcVelocity(&reference_points, points); - calcCurvature(&reference_points); - calcArcLength(&reference_points); - calcExtraPoints(&reference_points); - return reference_points; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return ref_points; } -void MPTOptimizer::calcOrientation(std::vector * ref_points) const +void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) const { - if (!ref_points) { - return; - } - for (std::size_t i = 0; i < ref_points->size(); i++) { - if (ref_points->at(i).fix_state) { - continue; + // if plan from ego + constexpr double epsilon = 1e-04; + const bool plan_from_ego = + mpt_param_.plan_from_ego && std::abs(current_ego_vel_) < epsilon && ref_points.size() > 1; + if (plan_from_ego) { + for (auto & ref_point : ref_points) { + ref_point.fix_kinematic_state = boost::none; } - if (i > 0) { - ref_points->at(i).q = - util::getQuaternionFromPoints(ref_points->at(i).p, ref_points->at(i - 1).p); - } else if (i == 0 && ref_points->size() > 1) { - ref_points->at(i).q = - util::getQuaternionFromPoints(ref_points->at(i + 1).p, ref_points->at(i).p); - } - ref_points->at(i).yaw = tf2::getYaw(ref_points->at(i).q); + /* + // 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 = + tier4_autoware_utils::findNearestIndex(ref_points, current_ego_pose_.position); + + // 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; + }(); + + 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::calcVelocity( - std::vector * ref_points, - const std::vector & points) const +std::vector MPTOptimizer::getFixedReferencePoints( + const std::vector & points, + const std::unique_ptr & prev_trajs) const { - if (!ref_points) { - return; - } - for (std::size_t i = 0; i < ref_points->size(); i++) { - ref_points->at(i).v = - points[util::getNearestIdx(points, ref_points->at(i).p)].longitudinal_velocity_mps; + 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(); } -} -void MPTOptimizer::calcCurvature(std::vector * ref_points) const -{ - if (!ref_points) { - return; + if (!mpt_param_.fix_points_around_ego) { + return std::vector(); } - int num_points = static_cast(ref_points->size()); - - /* calculate curvature by circle fitting from three points */ - geometry_msgs::msg::Point p1, p2, p3; - int max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); - int L = std::min(mpt_param_ptr_->num_curvature_sampling_points, max_smoothing_num); - for (int i = L; i < num_points - L; ++i) { - p1 = ref_points->at(i - L).p; - p2 = ref_points->at(i).p; - p3 = ref_points->at(i + L).p; - double den = std::max( - util::calculate2DDistance(p1, p2) * util::calculate2DDistance(p2, p3) * - util::calculate2DDistance(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; - if (!ref_points->at(i).fix_state) { - ref_points->at(i).k = curvature; + const auto & prev_ref_points = prev_trajs->mpt_ref_points; + const int nearest_prev_ref_idx = + tier4_autoware_utils::findNearestIndex(prev_ref_points, current_ego_pose_.position); + + // 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; + + return std::max(0, nearest_prev_ref_idx - backward_fixing_num); + }(); + + // 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 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); + }(); + + 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); + + if (!points_reaching_prev_points_flag) { + if (tier4_autoware_utils::calcSignedArcLength(points, 0, prev_ref_point.p) < 0) { + continue; + } + points_reaching_prev_points_flag = true; } - } - /* first and last curvature is copied from next value */ - for (int i = 0; i < std::min(L, num_points); ++i) { - if (!ref_points->at(i).fix_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_state) { - ref_points->at(num_points - i - 1).k = ref_points->at(std::max(num_points - L - 1, 0)).k; - } + ReferencePoint fixed_ref_point; + fixed_ref_point = prev_ref_point; + fixed_ref_point.fix_kinematic_state = prev_ref_point.optimized_kinematic_state; + + fixed_ref_points.push_back(fixed_ref_point); } + + return fixed_ref_points; } -void MPTOptimizer::calcArcLength(std::vector * ref_points) const +std::vector MPTOptimizer::getMPTFixedPoints( + const std::vector & ref_points) const { - if (!ref_points) { - return; - } - for (std::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 + util::calculate2DDistance(a, b); + std::vector mpt_fixed_traj; + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto & ref_point = ref_points.at(i); + + 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); + + 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); } } + + return mpt_fixed_traj; } -void MPTOptimizer::calcExtraPoints(std::vector * ref_points) const +// 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, std::shared_ptr debug_data_ptr) const { - if (!ref_points) { - return; + if (ref_points.empty()) { + return MPTMatrix{}; } - for (std::size_t i = 0; i < ref_points->size(); i++) { - const double p1_target_s = ref_points->at(i).s + mpt_param_ptr_->top_point_dist_from_base_link; - const int nearest_p1_idx = util::getNearestIdx(*ref_points, p1_target_s, i); - ref_points->at(i).top_pose.position = ref_points->at(nearest_p1_idx).p; - - const double p2_target_s = ref_points->at(i).s + mpt_param_ptr_->mid_point_dist_from_base_link; - const int nearest_p2_idx = util::getNearestIdx(*ref_points, p2_target_s, i); - ref_points->at(i).mid_pose.position = ref_points->at(nearest_p2_idx).p; - - ref_points->at(i).top_pose.orientation = - util::getQuaternionFromPoints(ref_points->at(i).top_pose.position, ref_points->at(i).p); - if (static_cast(i) == nearest_p1_idx && i > 0) { - ref_points->at(i).top_pose.orientation = - util::getQuaternionFromPoints(ref_points->at(i).p, ref_points->at(i - 1).p); - } else if (static_cast(i) == nearest_p1_idx) { - ref_points->at(i).top_pose.orientation = ref_points->at(i).q; - } - ref_points->at(i).mid_pose.orientation = ref_points->at(i).top_pose.orientation; - const double delta_yaw = - tf2::getYaw(ref_points->at(i).top_pose.orientation) - ref_points->at(i).yaw; - const double norm_delta_yaw = util::normalizeRadian(delta_yaw); - ref_points->at(i).delta_yaw_from_p1 = norm_delta_yaw; - ref_points->at(i).delta_yaw_from_p2 = ref_points->at(i).delta_yaw_from_p1; - } -} + stop_watch_.tic(__func__); -void MPTOptimizer::calcInitialState( - std::vector * ref_points, const geometry_msgs::msg::Pose & origin_pose) const -{ - if (!ref_points) { - return; - } + // NOTE: center offset of vehicle is always 0 in this algorithm. + // vehicle_model_ptr_->updateCenterOffset(0.0); - if (ref_points->empty()) { - return; - } + 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); - boost::optional begin_idx = boost::none; - double accum_s = 0; + Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); + Eigen::VectorXd Wex = Eigen::VectorXd::Zero(D_x * N_ref); - for (std::size_t i = 0; i < ref_points->size(); i++) { - double ds = 0.0; - if (i < ref_points->size() - 1) { - ds = ref_points->at(i + 1).s - ref_points->at(i).s; - } else if (i == ref_points->size() - 1 && ref_points->size() > 1) { - ds = ref_points->at(i).s - ref_points->at(i - 1).s; - } - accum_s += ds; - constexpr double max_s_for_prev_point = 3; - if (accum_s < max_s_for_prev_point && ref_points->at(i).fix_state) { - begin_idx = i; - break; + Eigen::MatrixXd Ad(D_x, D_x); + Eigen::MatrixXd Bd(D_x, D_u); + Eigen::MatrixXd Wd(D_x, 1); + + // 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; } - } - Eigen::VectorXd initial_state; - if (begin_idx) { - *ref_points = - std::vector{ref_points->begin() + begin_idx.get(), ref_points->end()}; - ref_points->front().optimized_state = ref_points->front().fix_state.get(); - } else { - ref_points->front().optimized_state = getState(origin_pose, ref_points->front()); - } -} + 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; -/* - * 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,...]) - */ -boost::optional MPTOptimizer::generateMPTMatrix( - const std::vector & reference_points, - const std::vector & path_points, - const std::unique_ptr & prev_trajs) const -{ - const int N = reference_points.size(); - const int DIM_X = vehicle_model_ptr_->getDimX(); - const int DIM_U = vehicle_model_ptr_->getDimU(); - const int DIM_Y = vehicle_model_ptr_->getDimY(); - - Eigen::MatrixXd Aex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_X); // state transition - Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_U * N); // control input - Eigen::MatrixXd Wex = Eigen::MatrixXd::Zero(DIM_X * N, 1); - Eigen::MatrixXd Cex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_X * N); - Eigen::MatrixXd Qex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_Y * N); - Eigen::MatrixXd R1ex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N); - Eigen::MatrixXd R2ex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N); - Eigen::MatrixXd Uref_ex = Eigen::MatrixXd::Zero(DIM_U * N, 1); - - /* weight matrix depends on the vehicle model */ - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_U, DIM_U); - Eigen::MatrixXd Q_adaptive = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - Eigen::MatrixXd R_adaptive = Eigen::MatrixXd::Zero(DIM_U, DIM_U); - Q(0, 0) = mpt_param_ptr_->lat_error_weight; - if (!prev_trajs) { - const double initial_lat_error_weight = mpt_param_ptr_->lat_error_weight * 1000; - Q(0, 0) = initial_lat_error_weight; - } - Q(1, 1) = mpt_param_ptr_->yaw_error_weight; - R(0, 0) = mpt_param_ptr_->steer_input_weight; - - Eigen::MatrixXd Ad(DIM_X, DIM_X); - Eigen::MatrixXd Bd(DIM_X, DIM_U); - Eigen::MatrixXd Wd(DIM_X, 1); - Eigen::MatrixXd Cd(DIM_Y, DIM_X); - Eigen::MatrixXd Uref(DIM_U, 1); - - geometry_msgs::msg::Pose last_ref_pose; - last_ref_pose.position = reference_points.back().p; - last_ref_pose.orientation = reference_points.back().q; - const auto last_extended_point = util::getLastExtendedPoint( - path_points.back(), last_ref_pose, traj_param_ptr_->delta_yaw_threshold_for_closest_point, - traj_param_ptr_->max_dist_for_extending_end_point); - - /* predict dynamics for N times */ - for (int i = 0; i < N; ++i) { - const double ref_k = reference_points[i].k; - double ds = 0.0; - if (i < N - 1) { - ds = reference_points[i + 1].s - reference_points[i].s; - } else if (i == N - 1 && N > 1) { - ds = reference_points[i].s - reference_points[i - 1].s; - } + 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; + }(); - /* get discrete state matrix A, B, C, W */ + // 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_->calculateDiscreteMatrix(&Ad, &Bd, &Cd, &Wd, ds); - - Q_adaptive = Q; - R_adaptive = R; - if (i == N - 1 && last_extended_point) { - Q_adaptive(0, 0) = mpt_param_ptr_->terminal_path_lat_error_weight; - Q_adaptive(1, 1) = mpt_param_ptr_->terminal_path_yaw_error_weight; - } else if (i == N - 1) { - Q_adaptive(0, 0) = mpt_param_ptr_->terminal_lat_error_weight; - Q_adaptive(1, 1) = mpt_param_ptr_->terminal_yaw_error_weight; - } + vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, ds); - /* update mpt matrix */ - int idx_x_i = i * DIM_X; - int idx_x_i_prev = (i - 1) * DIM_X; - int idx_u_i = i * DIM_U; - int idx_y_i = i * DIM_Y; - if (i == 0) { - Aex.block(0, 0, DIM_X, DIM_X) = Ad; - Bex.block(0, 0, DIM_X, DIM_U) = Bd; - Wex.block(0, 0, DIM_X, 1) = Wd; - } else { - Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X); - for (int j = 0; j < i; ++j) { - int idx_u_j = j * DIM_U; - Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) = - Ad * Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U); - } - Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd; - } - Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd; - Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd; - Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive; - R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive; - - /* get reference input (feed-forward) */ - vehicle_model_ptr_->calculateReferenceInput(&Uref); - if (std::fabs(Uref(0, 0)) < mpt_param_ptr_->zero_ff_steer_angle * M_PI / 180.0) { - Uref(0, 0) = 0.0; // ignore curvature noise + 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; + + 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); } - Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; - } - addSteerWeightR(&R1ex, reference_points); + Wex.segment(idx_x_i, D_x) = Ad * Wex.block(idx_x_i_prev, 0, D_x, 1) + Wd; + } MPTMatrix m; - m.Aex = Aex; m.Bex = Bex; m.Wex = Wex; - m.Cex = Cex; - m.Qex = Qex; - m.R1ex = R1ex; - m.R2ex = R2ex; - m.Uref_ex = Uref_ex; - if ( - m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() || - m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() || - m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any()) { - RCLCPP_WARN(rclcpp::get_logger("MPTOptimizer"), "[Avoidance] MPT matrix includes NaN."); - return boost::none; - } + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return m; } -void MPTOptimizer::addSteerWeightR( - Eigen::MatrixXd * R, const std::vector & ref_points) const +MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( + const std::vector & ref_points, + const std::vector & path_points, + std::shared_ptr debug_data_ptr) const { - const int N = ref_points.size(); - constexpr double DT = 0.1; - constexpr double ctrl_period = 0.03; - - /* add steering rate : weight for (u(i) - u(i-1) / dt )^2 */ - for (int i = 0; i < N - 1; ++i) { - const double steer_rate_r = mpt_param_ptr_->steer_rate_weight / (DT * DT); - (*R)(i + 0, i + 0) += steer_rate_r; - (*R)(i + 1, i + 0) -= steer_rate_r; - (*R)(i + 0, i + 1) -= steer_rate_r; - (*R)(i + 1, i + 1) += steer_rate_r; - } - if (N > 1) { - // steer rate i = 0 - (*R)(0, 0) += mpt_param_ptr_->steer_rate_weight / (ctrl_period * ctrl_period); + if (ref_points.empty()) { + return ValueMatrix{}; } - /* add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2 */ - const double steer_acc_r = mpt_param_ptr_->steer_acc_weight / std::pow(DT, 4); - const double steer_acc_r_cp1 = mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 3) * ctrl_period); - const double steer_acc_r_cp2 = - mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 2) * std::pow(ctrl_period, 2)); - const double steer_acc_r_cp4 = mpt_param_ptr_->steer_acc_weight / std::pow(ctrl_period, 4); - for (int i = 1; i < N - 1; ++i) { - (*R)(i - 1, i - 1) += (steer_acc_r); - (*R)(i - 1, i + 0) += (steer_acc_r * -2.0); - (*R)(i - 1, i + 1) += (steer_acc_r); - (*R)(i + 0, i - 1) += (steer_acc_r * -2.0); - (*R)(i + 0, i + 0) += (steer_acc_r * 4.0); - (*R)(i + 0, i + 1) += (steer_acc_r * -2.0); - (*R)(i + 1, i - 1) += (steer_acc_r); - (*R)(i + 1, i + 0) += (steer_acc_r * -2.0); - (*R)(i + 1, i + 1) += (steer_acc_r); - } - if (N > 1) { - // steer acc i = 1 - (*R)(0, 0) += steer_acc_r * 1.0 + steer_acc_r_cp2 * 1.0 + steer_acc_r_cp1 * 2.0; - (*R)(1, 0) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0; - (*R)(0, 1) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0; - (*R)(1, 1) += steer_acc_r * 1.0; - // steer acc i = 0 - (*R)(0, 0) += steer_acc_r_cp4 * 1.0; - } -} + stop_watch_.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_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); + + // update Q + Eigen::SparseMatrix Qex_sparse_mat(D_x * N_ref, D_x * N_ref); + std::vector> Qex_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 (near_kinematic_state_while_planning_from_ego) { + constexpr double obstacle_avoid_error_weight_ratio = 1 / 100.0; + return { + mpt_param_.obstacle_avoid_lat_error_weight * obstacle_avoid_error_weight_ratio, + mpt_param_.obstacle_avoid_yaw_error_weight * obstacle_avoid_error_weight_ratio}; + } else 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) { + 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}; + // } -void MPTOptimizer::addSteerWeightF(Eigen::VectorXd * f) const -{ - constexpr double DT = 0.1; - constexpr double ctrl_period = 0.03; - constexpr double raw_steer_cmd_prev = 0; - constexpr double raw_steer_cmd_pprev = 0; + return {mpt_param_.lat_error_weight, mpt_param_.yaw_error_weight}; + }(); - if (f->rows() < 2) { - return; - } + const double adaptive_lat_error_weight = adaptive_error_weight.at(0); + const double adaptive_yaw_error_weight = adaptive_error_weight.at(1); - // steer rate for i = 0 - (*f)(0) += -2.0 * mpt_param_ptr_->steer_rate_weight / (std::pow(DT, 2)) * 0.5; + Qex_triplet_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, adaptive_lat_error_weight)); + Qex_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()); + + // update R + Eigen::SparseMatrix Rex_sparse_mat(D_v, D_v); + std::vector> Rex_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( + Eigen::Triplet(D_x + D_u * i, D_x + D_u * i, adaptive_steer_weight)); + } + addSteerWeightR(Rex_triplet_vec, ref_points); - // const double steer_acc_r = mpt_param_.weight_steer_acc / std::pow(DT, 4); - const double steer_acc_r_cp1 = mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 3) * ctrl_period); - const double steer_acc_r_cp2 = - mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 2) * std::pow(ctrl_period, 2)); - const double steer_acc_r_cp4 = mpt_param_ptr_->steer_acc_weight / std::pow(ctrl_period, 4); + Rex_sparse_mat.setFromTriplets(Rex_triplet_vec.begin(), Rex_triplet_vec.end()); - // steer acc i = 0 - (*f)(0) += ((-2.0 * raw_steer_cmd_prev + raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5; + ValueMatrix m; + m.Qex = Qex_sparse_mat; + m.Rex = Rex_sparse_mat; - // steer acc for i = 1 - (*f)(0) += (-2.0 * raw_steer_cmd_prev * (steer_acc_r_cp1 + steer_acc_r_cp2)) * 0.5; - (*f)(1) += (2.0 * raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return m; } boost::optional MPTOptimizer::executeOptimization( - const bool enable_avoidance, const MPTMatrix & m, const std::vector & ref_points, - const std::vector & path_points, const CVMaps & maps, - DebugData * debug_data) + const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, + const std::vector & ref_points, std::shared_ptr debug_data_ptr) { - auto t_start1 = std::chrono::high_resolution_clock::now(); + if (ref_points.empty()) { + return Eigen::VectorXd{}; + } + + stop_watch_.tic(__func__); + + const size_t N_ref = ref_points.size(); - const auto x0 = ref_points.front().optimized_state; - ObjectiveMatrix obj_m = getObjectiveMatrix(x0, m); + // get matrix + ObjectiveMatrix obj_m = getObjectiveMatrix(mpt_mat, val_mat, ref_points, debug_data_ptr); ConstraintMatrix const_m = - getConstraintMatrix(enable_avoidance, x0, m, maps, ref_points, path_points, debug_data); + getConstraintMatrix(enable_avoidance, mpt_mat, ref_points, debug_data_ptr); + + // 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) { + const size_t seg_idx = tier4_autoware_utils::findNearestSegmentIndex( + prev_trajs->mpt_ref_points, ref_points.front().p); + double offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + prev_trajs->mpt_ref_points, seg_idx, ref_points.front().p); + + u0(0) = prev_trajs->mpt_ref_points.at(seg_idx).optimized_kinematic_state(0); + u0(1) = prev_trajs->mpt_ref_points.at(seg_idx).optimized_kinematic_state(1); + + for (size_t i = 0; i + 1 < N_ref; ++i) { + size_t prev_idx = seg_idx + i; + const size_t prev_N_ref = prev_trajs->mpt_ref_points.size(); + if (prev_idx + 2 > prev_N_ref) { + prev_idx = static_cast(prev_N_ref) - 2; + offset = 0.5; + } + + const double prev_val = prev_trajs->mpt_ref_points.at(prev_idx).optimized_input; + const double next_val = prev_trajs->mpt_ref_points.at(prev_idx + 1).optimized_input; + u0(D_x + i) = interpolation::lerp(prev_val, next_val, offset); + } + } + } + + 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(); - osqp_solver_ptr_ = std::make_unique( - obj_m.hessian, const_m.linear, obj_m.gradient, const_m.lower_bound, const_m.upper_bound, - 1.0e-3); - osqp_solver_ptr_->updateEpsRel(1.0e-3); + debug_data_ptr->msg_stream << " " + << "initOsqp" + << ":= " << stop_watch_.toc("initOsqp") << " [ms]\n"; + + // solve + stop_watch_.tic("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); + debug_data_ptr->msg_stream << " " + << "solveOsqp" + << ":= " << stop_watch_.toc("solveOsqp") << " [ms]\n"; - int solution_status = std::get<3>(result); + // check solution status + const int solution_status = std::get<3>(result); if (solution_status != 1) { - util::logOSQPSolutionStatus(solution_status); + utils::logOSQPSolutionStatus(solution_status); 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], ref_points.size()); + Eigen::Map(&result_vec[0], DIM_X + (N_ref - 1) * DIM_U); - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, "mpt opt time: = %f [ms]", - elapsed_ms1); - return optimized_control_variables; -} + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; -double MPTOptimizer::calcLateralError( - const geometry_msgs::msg::Point & target_point, const ReferencePoint & ref_point) const -{ - const double err_x = target_point.x - ref_point.p.x; - const double err_y = target_point.y - ref_point.p.y; - const double ref_yaw = tf2::getYaw(ref_point.q); - const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; - return lat_err; -} - -Eigen::VectorXd MPTOptimizer::getState( - const geometry_msgs::msg::Pose & target_pose, const ReferencePoint & nearest_ref_point) const -{ - const double lat_error = calcLateralError(target_pose.position, nearest_ref_point); - const double yaw_error = - util::normalizeRadian(tf2::getYaw(target_pose.orientation) - nearest_ref_point.yaw); - Eigen::VectorXd x0 = Eigen::VectorXd::Zero(3); - x0 << lat_error, yaw_error, std::atan(vehicle_param_ptr_->wheelbase * nearest_ref_point.k); - return x0; + 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; } -std::vector MPTOptimizer::getMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_matrix, - [[maybe_unused]] const std::vector & - optimized_points) const +MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( + const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, + [[maybe_unused]] const std::vector & ref_points, + std::shared_ptr debug_data_ptr) const { - const int DIM_X = vehicle_model_ptr_->getDimX(); - const auto x0 = ref_points.front().optimized_state; - Eigen::VectorXd Xex = mpt_matrix.Aex * x0 + mpt_matrix.Bex * Uex + mpt_matrix.Wex; + stop_watch_.tic(__func__); - std::vector traj_points; - { - const double lat_error = x0(0); - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose.position.x = ref_points[0].p.x - std::sin(ref_points[0].yaw) * lat_error; - traj_point.pose.position.y = ref_points[0].p.y + std::cos(ref_points[0].yaw) * lat_error; - traj_point.longitudinal_velocity_mps = ref_points.front().v; - traj_points.push_back(traj_point); - } + 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(); - for (std::size_t i = 1; i < ref_points.size(); ++i) { - const double lat_error = Xex((i - 1) * DIM_X); - Eigen::Vector3d state = Xex.segment((i - 1) * DIM_X, DIM_X); - setOptimizedState(&ref_points[i], state); - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose.position.x = ref_points[i].p.x - std::sin(ref_points[i].yaw) * lat_error; - traj_point.pose.position.y = ref_points[i].p.y + std::cos(ref_points[i].yaw) * lat_error; - traj_point.longitudinal_velocity_mps = ref_points[i].v; + const size_t D_xn = D_x * N_ref; + const size_t D_v = D_x + (N_ref - 1) * D_u; - traj_points.push_back(traj_point); - } - for (std::size_t i = 0; i < traj_points.size(); ++i) { - if (i > 0 && traj_points.size() > 1) { - traj_points[i].pose.orientation = util::getQuaternionFromPoints( - traj_points[i].pose.position, traj_points[i - 1].pose.position); - } else if (traj_points.size() > 1) { - traj_points[i].pose.orientation = util::getQuaternionFromPoints( - traj_points[i + 1].pose.position, traj_points[i].pose.position); - } else { - traj_points[i].pose.orientation = ref_points[i].q; - } - } - return traj_points; -} + // 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 + Eigen::SparseMatrix sparse_T_mat(D_xn, D_xn); + Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(D_xn); + std::vector> triplet_T_vec; + const double offset = mpt_param_.optimization_center_offset; -void MPTOptimizer::setOptimizedState( - ReferencePoint * ref_point, const Eigen::Vector3d & optimized_state) const -{ - ref_point->optimized_state = optimized_state; -} + for (size_t i = 0; i < N_ref; ++i) { + const double alpha = ref_points.at(i).alpha; -std::vector MPTOptimizer::getReferenceBounds( - const bool enable_avoidance, const std::vector & ref_points, const CVMaps & maps, - DebugData * debug_data) const -{ - std::vector ref_bounds; - std::vector debug_bounds_candidate_for_base_points; - std::vector debug_bounds_candidate_for_top_points; - std::vector debug_bounds_candidate_for_mid_points; - int cnt = 0; - for (const auto & point : ref_points) { - ReferencePoint ref_base_point; - ref_base_point.p = point.p; - ref_base_point.yaw = point.yaw; - - ReferencePoint ref_top_point; - ref_top_point.p = point.top_pose.position; - ref_top_point.yaw = tf2::getYaw(point.top_pose.orientation); - - ReferencePoint ref_mid_point; - ref_mid_point.p = point.mid_pose.position; - ref_mid_point.yaw = tf2::getYaw(point.mid_pose.orientation); - - geometry_msgs::msg::Pose debug_for_base_point; - debug_for_base_point.position = ref_base_point.p; - debug_for_base_point.orientation = point.q; - debug_bounds_candidate_for_base_points.push_back(debug_for_base_point); - - geometry_msgs::msg::Pose debug_for_top_point; - debug_for_top_point.position = ref_top_point.p; - debug_for_top_point.orientation = point.top_pose.orientation; - debug_bounds_candidate_for_top_points.push_back(debug_for_top_point); - - geometry_msgs::msg::Pose debug_for_mid_point; - debug_for_mid_point.position = ref_mid_point.p; - debug_for_mid_point.orientation = point.top_pose.orientation; - debug_bounds_candidate_for_mid_points.push_back(debug_for_mid_point); - - if ( - !util::transformMapToOptionalImage(ref_base_point.p, maps.map_info) || - !util::transformMapToOptionalImage(ref_mid_point.p, maps.map_info) || - !util::transformMapToOptionalImage(ref_top_point.p, maps.map_info)) { - Bounds bounds; - bounds.c0 = {1, -1}; - bounds.c1 = {1, -1}; - bounds.c2 = {1, -1}; - ref_bounds.emplace_back(bounds); - continue; - } + triplet_T_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, std::cos(alpha))); + triplet_T_vec.push_back(Eigen::Triplet(i * D_x, i * D_x + 1, offset * std::cos(alpha))); + triplet_T_vec.push_back(Eigen::Triplet(i * D_x + 1, i * D_x + 1, 1.0)); - // Calculate boundaries. - auto lat_bounds_0 = getBound(enable_avoidance, ref_base_point, maps); - auto lat_bounds_1 = getBound(enable_avoidance, ref_top_point, maps); - auto lat_bounds_2 = getBound(enable_avoidance, ref_mid_point, maps); - if (!lat_bounds_0 || !lat_bounds_1 || !lat_bounds_2) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, "Path is blocked at %i ", cnt); - Bounds bounds; - bounds.c0 = {1, -1}; - bounds.c1 = {1, -1}; - bounds.c2 = {1, -1}; - ref_bounds.emplace_back(bounds); - cnt++; - continue; - } - Bounds bounds; - bounds.c0 = lat_bounds_0.get(); - bounds.c1 = lat_bounds_1.get(); - bounds.c2 = lat_bounds_2.get(); - ref_bounds.emplace_back(bounds); - cnt++; + T_vec(i * D_x) = -offset * std::sin(alpha); } - debug_data->bounds = ref_bounds; - debug_data->bounds_candidate_for_base_points = debug_bounds_candidate_for_base_points; - debug_data->bounds_candidate_for_top_points = debug_bounds_candidate_for_top_points; - debug_data->bounds_candidate_for_mid_points = debug_bounds_candidate_for_mid_points; - return ref_bounds; -} + sparse_T_mat.setFromTriplets(triplet_T_vec.begin(), triplet_T_vec.end()); -boost::optional> MPTOptimizer::getBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const -{ - const auto rough_road_bound = getRoughBound(enable_avoidance, ref_point, maps); + 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; - if (!rough_road_bound) { - return boost::none; - } + // min J(v) = min (v'Hv + v'f) + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(D_v, D_v); + H.triangularView() = B.transpose() * QB + R; + H.triangularView() = H.transpose(); - std::vector broad_bound; - // initialize right bound with rough left bound - double rel_right_bound = rough_road_bound.get()[0]; - auto t_start = std::chrono::high_resolution_clock::now(); - float elapsed_ms = 0.0; - while (true) { - const auto bound_candidate = getBoundCandidate( - enable_avoidance, ref_point, maps, mpt_param_ptr_->clearance_from_road, - mpt_param_ptr_->clearance_from_object, rel_right_bound, rough_road_bound.get()); - - if (!bound_candidate) { - break; - } + // 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; - if (broad_bound.empty()) { - broad_bound = bound_candidate.get(); - } else { - const double bound_candidate_diff = - std::fabs(bound_candidate.get()[0] - bound_candidate.get()[1]); - const double broad_bound_diff = std::fabs(broad_bound[0] - broad_bound[1]); - if (bound_candidate_diff > broad_bound_diff) { - broad_bound = bound_candidate.get(); + 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; } - rel_right_bound = bound_candidate.get()[1]; - auto t_end = std::chrono::high_resolution_clock::now(); - elapsed_ms = std::chrono::duration(t_end - t_start).count(); - constexpr float max_ms = 10; - if (elapsed_ms > max_ms) { - // ROS_WARN_THROTTLE(1.0, "take too much time for calculating bound"); - return boost::none; + return 0; + }(); + const size_t N_second_slack = [&]() -> size_t { + if (mpt_param_.two_step_soft_constraint) { + return N_first_slack; } - } + return 0; + }(); - if (broad_bound.empty()) { - return boost::none; - } else { - return broad_bound; - } -} + // number of slack variables for one step + const size_t N_slack = N_first_slack + N_second_slack; -boost::optional> MPTOptimizer::getBoundCandidate( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps, - const double min_road_clearance, const double min_obj_clearance, const double rel_initial_lat, - const std::vector & rough_road_bound) const -{ - const bool search_expanding_side = true; - const auto left_bound = getValidLatError( - enable_avoidance, ref_point, rel_initial_lat, maps, min_road_clearance, min_obj_clearance, - rough_road_bound, search_expanding_side); - if (!left_bound) { - return boost::none; - } - constexpr double min_valid_lat_error = 0.1; - const double initial_right_bound = left_bound.get() - min_valid_lat_error; - const auto right_bound = getValidLatError( - enable_avoidance, ref_point, initial_right_bound, maps, min_road_clearance, min_obj_clearance, - rough_road_bound); - if (!right_bound) { - return boost::none; - } - if (std::fabs(left_bound.get() - right_bound.get()) < 1e-5) { - return boost::none; - } - std::vector bound{left_bound.get(), right_bound.get()}; - return bound; -} + // 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; -boost::optional> MPTOptimizer::getRoughBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const -{ - double left_bound = 0; - double right_bound = 0; - const double left_angle = util::normalizeRadian(ref_point.yaw + M_PI_2); - const double right_angle = util::normalizeRadian(ref_point.yaw - M_PI_2); - - geometry_msgs::msg::Point new_position; - new_position.x = ref_point.p.x; - new_position.y = ref_point.p.y; - auto original_clearance = getClearance(maps.clearance_map, new_position, maps.map_info); - auto original_object_clearance = - getClearance(maps.only_objects_clearance_map, new_position, maps.map_info); - if (!original_clearance || !original_object_clearance) { - return boost::none; - } - if (!enable_avoidance) { - original_object_clearance.emplace(std::numeric_limits::max()); - } - constexpr double min_road_clearance = 0.1; - constexpr double min_obj_clearance = 0.1; - if ( - original_clearance.get() > min_road_clearance && - original_object_clearance.get() > min_obj_clearance) { - const double initial_dist = 0; - right_bound = -1 * getTraversedDistance( - enable_avoidance, ref_point, right_angle, initial_dist, maps, - min_road_clearance, min_obj_clearance); - left_bound = getTraversedDistance( - enable_avoidance, ref_point, left_angle, initial_dist, maps, min_road_clearance, - min_obj_clearance); - } else { - const double initial_dist = 0; - const bool search_expanding_side = true; - const double right_s = getTraversedDistance( - enable_avoidance, ref_point, right_angle, initial_dist, maps, min_road_clearance, - min_obj_clearance, search_expanding_side); - const double left_s = getTraversedDistance( - enable_avoidance, ref_point, left_angle, initial_dist, maps, min_road_clearance, - min_obj_clearance, search_expanding_side); - if (left_s < right_s) { - // Pick left side: - right_bound = left_s; - left_bound = getTraversedDistance( - enable_avoidance, ref_point, left_angle, left_s, maps, min_road_clearance, - min_obj_clearance); - } else { - // Pick right side: - left_bound = -right_s; - right_bound = -getTraversedDistance( - enable_avoidance, ref_point, right_angle, right_s, maps, min_road_clearance, - min_obj_clearance); - } - } - if (std::fabs(left_bound - right_bound) < 1e-6) { - return boost::none; - } - std::vector bound{left_bound, right_bound}; - return bound; -} + // extend f for slack variables + Eigen::VectorXd full_f(D_v + N_ref * N_slack); -boost::optional MPTOptimizer::getClearance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const auto image_point = util::transformMapToOptionalImage(map_point, map_info); - if (!image_point) { - return boost::none; + 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); } - const float clearance = clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - return clearance; -} -ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( - const Eigen::VectorXd & x0, const MPTMatrix & m) const -{ - const int DIM_U_N = m.Uref_ex.rows(); - const Eigen::MatrixXd CB = m.Cex * m.Bex; - const Eigen::MatrixXd QCB = m.Qex * CB; - // Eigen::MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; - // This calculation is heavy. looking for a good way. - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(DIM_U_N, DIM_U_N); - H.triangularView() = CB.transpose() * QCB; - H.triangularView() += m.R1ex + m.R2ex; - H.triangularView() = H.transpose(); - Eigen::VectorXd f = - (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex; - addSteerWeightF(&f); - - constexpr int num_lat_constraint = 3; - const int num_objective_variables = DIM_U_N * (1 + num_lat_constraint); - Eigen::MatrixXd extend_h = Eigen::MatrixXd::Zero(DIM_U_N, DIM_U_N); - Eigen::VectorXd extend_f = Eigen::VectorXd::Ones(DIM_U_N); - Eigen::MatrixXd concat_h = - Eigen::MatrixXd::Zero(num_objective_variables, num_objective_variables); - Eigen::VectorXd concat_f = Eigen::VectorXd::Zero(num_objective_variables); - concat_h.block(0, 0, DIM_U_N, DIM_U_N) = H; - concat_h.block(DIM_U_N, DIM_U_N, DIM_U_N, DIM_U_N) = extend_h; - concat_h.block(DIM_U_N * 2, DIM_U_N * 2, DIM_U_N, DIM_U_N) = extend_h; - concat_h.block(DIM_U_N * 3, DIM_U_N * 3, DIM_U_N, DIM_U_N) = extend_h; - concat_f << f, mpt_param_ptr_->base_point_weight * extend_f, - mpt_param_ptr_->top_point_weight * extend_f, mpt_param_ptr_->mid_point_weight * extend_f; ObjectiveMatrix obj_matrix; - obj_matrix.hessian = concat_h; - obj_matrix.gradient = {concat_f.data(), concat_f.data() + concat_f.rows()}; + obj_matrix.hessian = full_H; + obj_matrix.gradient = full_f; + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return obj_matrix; } @@ -894,368 +890,737 @@ ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( // Set constraint: lb <= Ax <= ub // decision variable // x := [u0, ..., uN-1 | z00, ..., z0N-1 | z10, ..., z1N-1 | z20, ..., z2N-1] -// \in \mathbb{R}^{N * (N_point + 1)} -ConstraintMatrix MPTOptimizer::getConstraintMatrix( - const bool enable_avoidance, const Eigen::VectorXd & x0, const MPTMatrix & m, const CVMaps & maps, +// \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]] const std::vector & path_points, - DebugData * debug_data) const + [[maybe_unused]] std::shared_ptr debug_data_ptr) const { - std::vector dist_vec{ - mpt_param_ptr_->base_point_dist_from_base_link, mpt_param_ptr_->top_point_dist_from_base_link, - mpt_param_ptr_->mid_point_dist_from_base_link}; - - const size_t N_ref = m.Uref_ex.rows(); - const size_t N_state = vehicle_model_ptr_->getDimX(); - const size_t N_point = dist_vec.size(); - const size_t N_dec = N_ref * (N_point + 1); - - const auto bounds = getReferenceBounds(enable_avoidance, ref_points, maps, debug_data); - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * N_ref * N_point + N_ref, N_dec); - Eigen::VectorXd lb = - Eigen::VectorXd::Constant(3 * N_ref * N_point + N_ref, -autoware::common::osqp::INF); - Eigen::VectorXd ub = - Eigen::VectorXd::Constant(3 * N_ref * N_point + N_ref, autoware::common::osqp::INF); - - // Define constraint matrices and vectors - // Gap from reference point around vehicle base_link - { - // C := [I | O | O] - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); - for (size_t i = 0; i < N_ref; ++i) { - C(i, N_state * i) = 1; - } - // bias := Cast * (Aex * x0 + Wex) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); - // A_blk := [C * Bex | I | O | O - // -C * Bex | I | O | O - // O | I | O | O] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec); - A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex; - A_blk.block(0, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex; - A_blk.block(N_ref, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb_blk := [-bias + bounds.lb - // bias - bounds.ub - // 0] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - lb_blk(i) = -bias(i) + bounds[i].c0.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i].c0.ub; - } else { - lb_blk(i) = -bias(i) + bounds[i + 1].c0.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c0.ub; - } - } - // Assign - A.block(0, 0, 3 * N_ref, N_dec) = A_blk; - lb.segment(0, 3 * N_ref) = lb_blk; - } + stop_watch_.tic(__func__); - // Gap from reference point around vehicle top - { - // C := diag([cos(alpha1) | l1*cos(alpha1) | 0]) - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); - for (size_t i = 0; i < N_ref; ++i) { - Eigen::MatrixXd Cast = Eigen::MatrixXd::Zero(1, N_state); - if (i == N_ref - 1) { - Cast(0, 0) = std::cos(ref_points[i].delta_yaw_from_p1); - Cast(0, 1) = dist_vec[1] * std::cos(ref_points[i].delta_yaw_from_p1); - } else { - Cast(0, 0) = std::cos(ref_points[i + 1].delta_yaw_from_p1); - Cast(0, 1) = dist_vec[1] * std::cos(ref_points[i + 1].delta_yaw_from_p1); + // 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 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_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; } - C.block(i, N_state * i, 1, N_state) = Cast; + return N_avoid; } - // bias := Cast * (Aex * x0 + Wex) - l1 * sin(alpha1) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - bias(i) -= dist_vec[1] * std::sin(ref_points[i].delta_yaw_from_p1); - } else { - bias(i) -= dist_vec[1] * std::sin(ref_points[i + 1].delta_yaw_from_p1); - } + 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; } - // A_blk := [C * Bex | O | I | O - // -C * Bex | O | I | O - // O | O | I | O] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec); - A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex; - A_blk.block(0, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex; - A_blk.block(N_ref, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb_blk := [-bias + bounds.lb - // bias - bounds.ub - // 0] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - lb_blk(i) = -bias(i) + bounds[i].c1.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i].c1.ub; - } else { - lb_blk(i) = -bias(i) + bounds[i + 1].c1.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c1.ub; - } + 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 + }(); + + // 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) { + fixed_points_indices.push_back(i); } - // Assign - A.block(3 * N_ref, 0, 3 * N_ref, N_dec) = A_blk; - lb.segment(3 * N_ref, 3 * N_ref) = lb_blk; } - // Gap from reference point around vehicle middle - { - // C := [diag(cos(alpha2)) | diag(l2*cos(alpha2)) | O] - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); + + // calculate rows 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; + } + if (mpt_param_.hard_constraint) { + A_rows += N_ref * N_avoid; + } + A_rows += fixed_points_indices.size() * D_x; + if (mpt_param_.steer_limit_constraint) { + A_rows += N_u; + } + + 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] + 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) { - Eigen::MatrixXd Cast = Eigen::MatrixXd::Zero(1, N_state); - if (i == N_ref - 1) { - Cast(0, 0) = std::cos(ref_points[i].delta_yaw_from_p2); - Cast(0, 1) = dist_vec[2] * std::cos(ref_points[i].delta_yaw_from_p2); - } else { - Cast(0, 0) = std::cos(ref_points[i + 1].delta_yaw_from_p2); - Cast(0, 1) = dist_vec[2] * std::cos(ref_points[i + 1].delta_yaw_from_p2); - } - C.block(i, N_state * i, 1, N_state) = Cast; + 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); + + 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); } - // bias := Cast * (Aex * x0 + Wex) - l2 * sin(alpha2) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - bias(i) -= dist_vec[2] * std::sin(ref_points[i].delta_yaw_from_p2); - } else { - bias(i) -= dist_vec[2] * std::sin(ref_points[i + 1].delta_yaw_from_p2); + 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; + + // calculate bounds + const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx); + + // 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; + + 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; } } - // A_blk := [C * Bex | O | O | I - // -C * Bex | O | O | I - // O | O | O | I] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec); - A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex; - A_blk.block(0, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex; - A_blk.block(N_ref, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb_blk := [-bias + bounds.lb - // bias - bounds.ub - // 0] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - lb_blk(i) = -bias(i) + bounds[i].c2.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i].c2.ub; - } else { - lb_blk(i) = -bias(i) + bounds[i + 1].c2.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c2.ub; - } + + // hard constraints + if (mpt_param_.hard_constraint) { + const size_t A_blk_rows = N_ref; + + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); + A_blk.block(0, 0, N_ref, N_ref) = CB; + + A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; + lb.segment(A_rows_end, A_blk_rows) = part_lb - CW; + ub.segment(A_rows_end, A_blk_rows) = part_ub - CW; + + A_rows_end += A_blk_rows; } - // Assign - A.block(2 * 3 * N_ref, 0, 3 * N_ref, N_dec) = A_blk; - lb.segment(2 * 3 * N_ref, 3 * N_ref) = lb_blk; } - // Fixed points constraint - { - // C := [I | O | O] - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); - for (size_t i = 0; i < N_ref; ++i) { - C(i, N_state * i) = 1; - } - // bias := Cast * (Aex * x0 + Wex) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); + // 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); - // Assign - A.block(3 * N_point * N_ref, 0, N_ref, N_ref) = C * m.Bex; - for (size_t i = 0; i < N_ref; ++i) { - if (ref_points[i + 1].fix_state && i + 1 < N_ref) { - lb(3 * N_point * N_ref + i) = ref_points[i + 1].fix_state.get()(0) - bias(i); - ub(3 * N_point * N_ref + i) = ref_points[i + 1].fix_state.get()(0) - bias(i); - } else if (i == ref_points.size() - 1 && mpt_param_ptr_->is_hard_fixing_terminal_point) { - lb(3 * N_point * N_ref + i) = -bias(i); - ub(3 * N_point * N_ref + i) = -bias(i); - } + 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); + + A_rows_end += D_x; } } - ConstraintMatrix constraint_matrix; - - constraint_matrix.linear = A; + // steer max 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); - for (int i = 0; i < lb.size(); ++i) { - constraint_matrix.lower_bound.push_back(lb(i)); - constraint_matrix.upper_bound.push_back(ub(i)); + A_rows_end += N_u; } + ConstraintMatrix constraint_matrix; + constraint_matrix.linear = A; + constraint_matrix.lower_bound = lb; + constraint_matrix.upper_bound = ub; + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return constraint_matrix; } -std::vector MPTOptimizer::getBaseReferencePoints( - const std::vector & interpolated_points, - const std::unique_ptr & prev_trajs, DebugData * debug_data) const +std::vector MPTOptimizer::getMPTPoints( + std::vector & fixed_ref_points, + std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, + const MPTMatrix & mpt_mat, std::shared_ptr debug_data_ptr) { - std::vector reference_points; - for (const auto & e : interpolated_points) { - ReferencePoint ref_point; - ref_point.p = e; - reference_points.push_back(ref_point); - } - if (!prev_trajs) { - return reference_points; + 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); + + lat_error_vec.push_back(ref_point.fix_kinematic_state.get()(0)); + yaw_error_vec.push_back(ref_point.fix_kinematic_state.get()(1)); } - if (prev_trajs->model_predictive_trajectory.size() != prev_trajs->mpt_ref_points.size()) { - return reference_points; + + 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)); } - // re-calculating points' position for fixing - std::vector cropped_interpolated_points; - double accum_s_for_interpolated = 0; - for (std::size_t i = 0; i < interpolated_points.size(); i++) { - if (i > 0) { - accum_s_for_interpolated += - util::calculate2DDistance(interpolated_points[i], interpolated_points[i - 1]); + // calculate trajectory from optimization result + std::vector traj_points; + debug_data_ptr->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_ptr->mpt_ref_poses.push_back(ref_pose); + debug_data_ptr->lateral_errors.push_back(lat_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); + } } - if ( - accum_s_for_interpolated > - traj_param_ptr_->num_sampling_points * traj_param_ptr_->delta_arc_length_for_mpt_points) { - break; + + autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0); + + traj_point.longitudinal_velocity_mps = ref_point.v; + 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; + + 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); + + vehicle_circle_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + ref_point.alpha); + + debug_data_ptr->vehicle_circles_pose.at(i).push_back(vehicle_circle_pose); + } } - cropped_interpolated_points.push_back(interpolated_points[i]); } - constexpr double fine_resolution = 0.005; - std::vector fine_interpolated_points; - fine_interpolated_points = - util::getInterpolatedPoints(cropped_interpolated_points, fine_resolution); - const double max_s = - traj_param_ptr_->backward_fixing_distance + traj_param_ptr_->forward_fixing_mpt_distance; - const int num_points = std::min( - static_cast(reference_points.size()), - static_cast(max_s / traj_param_ptr_->delta_arc_length_for_mpt_points)); - - for (int i = 0; i < num_points; i++) { - const int nearest_prev_idx = - util::getNearestPointIdx(prev_trajs->mpt_ref_points, reference_points[i].p); - if ( - util::calculate2DDistance( - prev_trajs->mpt_ref_points[nearest_prev_idx].p, reference_points[i].p) >= - std::fabs(traj_param_ptr_->delta_arc_length_for_mpt_points)) { + + // 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_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + + return traj_points; +} + +void MPTOptimizer::calcOrientation(std::vector & ref_points) const +{ + const auto yaw_angles = slerpYawFromReferencePoints(ref_points); + for (size_t i = 0; i < ref_points.size(); ++i) { + if (ref_points.at(i).fix_kinematic_state) { continue; } - const int nearest_idx = - util::getNearestIdx(fine_interpolated_points, prev_trajs->mpt_ref_points[nearest_prev_idx].p); - if ( - util::calculate2DDistance( - fine_interpolated_points[nearest_idx], prev_trajs->mpt_ref_points[nearest_prev_idx].p) < - fine_resolution) { - reference_points[i] = prev_trajs->mpt_ref_points[nearest_prev_idx]; - reference_points[i].fix_state = prev_trajs->mpt_ref_points[nearest_prev_idx].optimized_state; + + ref_points.at(i).yaw = yaw_angles.at(i); + } +} + +void MPTOptimizer::calcVelocity( + std::vector & ref_points, + const std::vector & points) const +{ + for (size_t i = 0; i < ref_points.size(); i++) { + ref_points.at(i).v = points[tier4_autoware_utils::findNearestIndex(points, ref_points.at(i).p)] + .longitudinal_velocity_mps; + } +} + +void MPTOptimizer::calcCurvature(std::vector & 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); + } + } + // 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; } } +} - std::vector trimmed_reference_points; - for (std::size_t i = 0; i < reference_points.size(); i++) { +void MPTOptimizer::calcArcLength(std::vector & ref_points) const +{ + for (size_t i = 0; i < ref_points.size(); i++) { if (i > 0) { - const double dx = reference_points[i].p.x - reference_points[i - 1].p.x; - const double dy = reference_points[i].p.y - reference_points[i - 1].p.y; - if (std::fabs(dx) < 1e-6 && std::fabs(dy) < 1e-6) { - continue; + 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); + } + } +} + +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 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); + + // 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; + + 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); + + 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; + }(); + + // 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 size_t prev_idx = + tier4_autoware_utils::findNearestIndex(prev_ref_points, ref_points.at(i).p); + 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; } } - trimmed_reference_points.push_back(reference_points[i]); } +} + +void MPTOptimizer::addSteerWeightR( + std::vector> & Rex_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 = 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)); + } +} - for (const auto & e : trimmed_reference_points) { - if (e.fix_state) { - geometry_msgs::msg::Point rel_point; - rel_point.y = e.fix_state.get()(0); - geometry_msgs::msg::Pose origin; - origin.position = e.p; - origin.orientation = e.q; - geometry_msgs::msg::Pose debug_pose; - debug_pose.position = util::transformToAbsoluteCoordinate2D(rel_point, origin); - debug_data->fixed_mpt_points.push_back(debug_pose); - continue; +void MPTOptimizer::calcBounds( + std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + // search bounds candidate for each ref points + SequentialBoundsCandidates sequential_bounds_candidates; + for (const auto & ref_point : ref_points) { + const auto bounds_candidates = getBoundsCandidates( + enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data_ptr); + sequential_bounds_candidates.push_back(bounds_candidates); + } + + // search continuous and widest bounds only for front point + for (size_t i = 0; i < sequential_bounds_candidates.size(); ++i) { + // NOTE: back() is the front avoiding circle + const auto & bounds_candidates = sequential_bounds_candidates.at(i); + const auto & ref_point = ref_points.at(i); + + // extract only continuous bounds; + if (i == 0) { // TODO(murooka) use previous bounds, not widest bounds + const auto & widest_bounds = findWidestBounds(bounds_candidates); + ref_points.at(i).bounds = widest_bounds; + } else { + const auto & prev_ref_point = ref_points.at(i - 1); + const auto & prev_continuous_bounds = prev_ref_point.bounds; + + // search continuous bounds + double max_length = std::numeric_limits::min(); + int max_idx = -1; + for (size_t c_idx = 0; c_idx < bounds_candidates.size(); ++c_idx) { + const auto & bounds_candidate = bounds_candidates.at(c_idx); + const double overlapped_length = calcOverlappedBounds( + convertRefPointsToPose(ref_point), bounds_candidate, + convertRefPointsToPose(prev_ref_point), prev_continuous_bounds); + if (overlapped_length > 0 && max_length < overlapped_length) { + max_length = overlapped_length; + max_idx = c_idx; + } + } + + // find widest bounds + if (max_idx == -1) { + // NOTE: set invalid bounds so that MPT won't be solved + // TODO(murooka) this invalid bounds even makes optimization solved. consider if this is ok + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger("getBounds: front"), is_showing_debug_info_, "invalid bounds"); + const auto invalid_bounds = + Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; + ref_points.at(i).bounds = invalid_bounds; + } else { + ref_points.at(i).bounds = bounds_candidates.at(max_idx); + } } } - return trimmed_reference_points; + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return; } -double MPTOptimizer::getTraversedDistance( - const bool enable_avoidance, const ReferencePoint & ref_point, const double traverse_angle, - const double initial_value, const CVMaps & maps, const double min_road_clearance, - const double min_obj_clearance, const bool search_expanding_side) const +void MPTOptimizer::calcVehicleBounds( + std::vector & ref_points, [[maybe_unused]] const CVMaps & maps, + std::shared_ptr debug_data_ptr, [[maybe_unused]] const bool enable_avoidance) const { - constexpr double ds = 0.1; - constexpr double lane_width = 7.5; - int n = static_cast(lane_width / ds); - - double traversed_dist = initial_value; - for (int i = 0; i < n; ++i) { - traversed_dist += ds; - geometry_msgs::msg::Point new_position; - new_position.x = ref_point.p.x + traversed_dist * std::cos(traverse_angle); - new_position.y = ref_point.p.y + traversed_dist * std::sin(traverse_angle); - auto clearance = getClearance(maps.clearance_map, new_position, maps.map_info); - auto obj_clearance = getClearance(maps.only_objects_clearance_map, new_position, maps.map_info); - if (!clearance || !obj_clearance) { - clearance.emplace(0); - obj_clearance.emplace(0); + stop_watch_.tic(__func__); + + 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); } - if (!enable_avoidance) { - obj_clearance.emplace(std::numeric_limits::max()); + return; + } + + SplineInterpolationPoints2d ref_points_spline_interpolation; + ref_points_spline_interpolation.calcSplineCoefficients(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(); + + 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); + + const double offset_y = -tier4_autoware_utils::calcDistance2d(ref_point, avoid_traj_pose) * + std::sin(avoid_yaw - vehicle_bounds_pose_yaw); + + const auto vehicle_bounds_pose = + tier4_autoware_utils::calcOffsetPose(avoid_traj_pose, 0.0, offset_y, 0.0); + + // interpolate bounds + const double avoid_s = ref_points_spline_interpolation.getAccumulatedLength(p_idx) + d; + for (size_t cp_idx = p_idx; 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; + } + + 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); + + const auto prev_bounds = ref_points.at(prev_avoid_idx).bounds; + const auto next_bounds = ref_points.at(prev_avoid_idx + 1).bounds; + + auto bounds = Bounds::lerp(prev_bounds, next_bounds, ratio); + bounds.translate(offset_y); + + ref_points.at(p_idx).vehicle_bounds.push_back(bounds); + break; + } + + if (cp_idx == ref_points.size() - 1) { + ref_points.at(p_idx).vehicle_bounds.push_back(ref_points.back().bounds); + } + } + + ref_points.at(p_idx).vehicle_bounds_poses.push_back(vehicle_bounds_pose); } - if (search_expanding_side) { - if (clearance.get() > min_road_clearance && obj_clearance.get() > min_obj_clearance) { - break; + } + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; +} + +BoundsCandidates MPTOptimizer::getBoundsCandidates( + const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, const CVMaps & maps, + [[maybe_unused]] std::shared_ptr debug_data_ptr) const +{ + BoundsCandidates bounds_candidate; + + constexpr double max_search_lane_width = 5.0; + const std::vector ds_vec{0.45, 0.15, 0.05, 0.01}; + + // search right to left + const double bound_angle = + tier4_autoware_utils::normalizeRadian(tf2::getYaw(avoiding_point.orientation) + M_PI_2); + + double traversed_dist = -max_search_lane_width; + double current_right_bound = -max_search_lane_width; + + // calculate the initial position is empty or not + // 0.drivable, 1.out of map 2.out of road or object + CollisionType previous_collision_type = + getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); + + const auto has_collision = [&](const CollisionType & collision_type) -> bool { + return collision_type == CollisionType::OUT_OF_ROAD || collision_type == CollisionType::OBJECT; + }; + CollisionType latest_right_bound_collision_type = CollisionType::OUT_OF_ROAD; + + while (traversed_dist < max_search_lane_width) { + for (size_t ds_idx = 0; ds_idx < ds_vec.size(); ++ds_idx) { + const double ds = ds_vec.at(ds_idx); + while (true) { + const CollisionType current_collision_type = + getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); + + // return only full bounds whenever finding out of map + if (current_collision_type == CollisionType::OUT_OF_SIGHT) { + const auto full_bounds = Bounds{ + -max_search_lane_width, max_search_lane_width, CollisionType::OUT_OF_SIGHT, + CollisionType::OUT_OF_SIGHT}; + return BoundsCandidates({full_bounds}); + } + + if (has_collision(previous_collision_type)) { + if (!has_collision(current_collision_type)) { // if target_position becomes no collision + if (ds_idx == ds_vec.size() - 1) { + current_right_bound = traversed_dist - ds / 2.0; + latest_right_bound_collision_type = previous_collision_type; + previous_collision_type = current_collision_type; + } + break; + } + } else { + if (has_collision(current_collision_type)) { + if (ds_idx == ds_vec.size() - 1) { + const double left_bound = traversed_dist - ds / 2.0; + bounds_candidate.push_back(Bounds{ + current_right_bound, left_bound, latest_right_bound_collision_type, + current_collision_type}); + previous_collision_type = current_collision_type; + } + break; + } + } + + // if target_position is out of lane + if (traversed_dist >= max_search_lane_width) { + if (!has_collision(previous_collision_type) && ds_idx == ds_vec.size() - 1) { + const double left_bound = traversed_dist - ds / 2.0; + bounds_candidate.push_back(Bounds{ + current_right_bound, left_bound, latest_right_bound_collision_type, + CollisionType::OUT_OF_ROAD}); + } + break; + } + + // go forward with ds + traversed_dist += ds; + previous_collision_type = current_collision_type; } - } else { - if (clearance.get() < min_road_clearance || obj_clearance.get() < min_obj_clearance) { - break; + + if (ds_idx != ds_vec.size() - 1) { + // go back with ds since target_position became empty or road/object + // NOTE: if ds is the last of ds_vec, don't have to go back + traversed_dist -= ds; } } } - return traversed_dist; + + // if empty + // TODO(murooka) sometimes this condition realizes + if (bounds_candidate.empty()) { + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger("getBoundsCandidate"), is_showing_debug_info_, "empty bounds candidate"); + // NOTE: set invalid bounds so that MPT won't be solved + const auto invalid_bounds = + Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; + bounds_candidate.push_back(invalid_bounds); + } + + return bounds_candidate; } -boost::optional MPTOptimizer::getValidLatError( - const bool enable_avoidance, const ReferencePoint & ref_point, const double initial_value, - const CVMaps & maps, const double min_road_clearance, const double min_obj_clearance, - const std::vector & rough_road_bound, const bool search_expanding_side) const +// 0.drivable, 1.out of drivable area 2.out of road or object +// 0.NO_COLLISION, 1.OUT_OF_SIGHT, 2.OUT_OF_ROAD, 3.OBJECT +CollisionType MPTOptimizer::getCollisionType( + const CVMaps & maps, const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, + const double traversed_dist, const double bound_angle) const { - constexpr double ds = 0.01; - constexpr double lane_width = 7.5; - int n = static_cast(lane_width / ds); - - double rel_value = initial_value; - for (int i = 0; i < n; ++i) { - rel_value -= ds; - if (rel_value > rough_road_bound[0] || rel_value < rough_road_bound[1]) { - return boost::none; - } - geometry_msgs::msg::Point rel_point; - rel_point.y = rel_value; - geometry_msgs::msg::Pose origin; - origin.position = ref_point.p; - origin.orientation = util::getQuaternionFromYaw(ref_point.yaw); - const auto new_position = util::transformToAbsoluteCoordinate2D(rel_point, origin); - auto clearance = getClearance(maps.clearance_map, new_position, maps.map_info); - auto obj_clearance = getClearance(maps.only_objects_clearance_map, new_position, maps.map_info); - if (!clearance || !obj_clearance) { - return boost::none; + // calculate clearance + const double min_soft_road_clearance = mpt_param_.vehicle_circle_radius + + mpt_param_.soft_clearance_from_road + + mpt_param_.extra_desired_clearance_from_road; + const double min_obj_clearance = mpt_param_.vehicle_circle_radius + + mpt_param_.clearance_from_object + + mpt_param_.soft_clearance_from_road; + + // calculate target position + const geometry_msgs::msg::Point target_pos = tier4_autoware_utils::createPoint( + avoiding_point.position.x + traversed_dist * std::cos(bound_angle), + avoiding_point.position.y + traversed_dist * std::sin(bound_angle), 0.0); + + const auto opt_road_clearance = getClearance(maps.clearance_map, target_pos, maps.map_info); + const auto opt_obj_clearance = + getClearance(maps.only_objects_clearance_map, target_pos, maps.map_info); + + // object has more priority than road, so its condition exists first + if (enable_avoidance && opt_obj_clearance) { + const bool is_obj = opt_obj_clearance.get() < min_obj_clearance; + if (is_obj) { + return CollisionType::OBJECT; } - if (!enable_avoidance) { - obj_clearance.emplace(std::numeric_limits::max()); - } - if (search_expanding_side) { - if (clearance.get() > min_road_clearance && obj_clearance.get() > min_obj_clearance) { - break; - } + } + + if (opt_road_clearance) { + const bool out_of_road = opt_road_clearance.get() < min_soft_road_clearance; + if (out_of_road) { + return CollisionType::OUT_OF_ROAD; } else { - if (clearance.get() < min_road_clearance || obj_clearance.get() < min_obj_clearance) { - break; - } + return CollisionType::NO_COLLISION; } } - return rel_value; + + return CollisionType::OUT_OF_SIGHT; +} + +boost::optional MPTOptimizer::getClearance( + const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & map_info) const +{ + const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); + if (!image_point) { + return boost::none; + } + const float clearance = clearance_map.ptr(static_cast( + image_point.get().y))[static_cast(image_point.get().x)] * + map_info.resolution; + return clearance; } diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 68064d2be7896..78abaf7ca50c5 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -14,32 +14,20 @@ #include "obstacle_avoidance_planner/node.hpp" -#include "obstacle_avoidance_planner/debug.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "obstacle_avoidance_planner/cv_utils.hpp" +#include "obstacle_avoidance_planner/debug_visualization.hpp" +#include "obstacle_avoidance_planner/utils.hpp" +#include "rclcpp/time.hpp" +#include "tf2/utils.h" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/trajectory/tmp_conversion.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 @@ -48,25 +36,175 @@ #include #include +namespace +{ +template +size_t searchExtendedZeroVelocityIndex( + const std::vector & fine_points, const std::vector & vel_points) +{ + const auto opt_zero_vel_idx = tier4_autoware_utils::searchZeroVelocityIndex(vel_points); + const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; + return tier4_autoware_utils::findNearestIndex( + fine_points, vel_points.at(zero_vel_idx).pose.position); +} + +bool isPathShapeChanged( + const geometry_msgs::msg::Pose & ego_pose, + const std::vector & path_points, + const std::unique_ptr> & + prev_path_points, + const double max_mpt_length, const double max_path_shape_change_dist, + const double delta_yaw_threshold) +{ + if (!prev_path_points) { + return false; + } + + // truncate prev points from ego pose to fixed end points + const auto opt_prev_begin_idx = tier4_autoware_utils::findNearestIndex( + *prev_path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); + const size_t prev_begin_idx = opt_prev_begin_idx ? *opt_prev_begin_idx : 0; + const auto truncated_prev_points = + points_utils::clipForwardPoints(*prev_path_points, prev_begin_idx, max_mpt_length); + + // truncate points from ego pose to fixed end points + const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); + const size_t begin_idx = opt_begin_idx ? *opt_begin_idx : 0; + const auto truncated_points = + points_utils::clipForwardPoints(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 = + tier4_autoware_utils::calcLateralOffset(truncated_points, prev_point.pose.position); + if (dist > max_path_shape_change_dist) { + return true; + } + } + + return false; +} + +bool isPathGoalChanged( + const double current_vel, + const std::vector & path_points, + const std::unique_ptr> & + prev_path_points) +{ + if (!prev_path_points) { + return false; + } + + constexpr double min_vel = 1e-3; + if (std::abs(current_vel) > min_vel) { + return false; + } + + // 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(path_points.back(), prev_path_points->back()); + if (goal_moving_dist < max_goal_moving_dist) { + return false; + } + + return true; +} + +bool hasValidNearestPointFromEgo( + const geometry_msgs::msg::Pose & ego_pose, const Trajectories & trajs, + const TrajectoryParam & traj_param) +{ + const auto traj = trajs.model_predictive_trajectory; + const auto interpolated_points = + interpolation_utils::getInterpolatedPoints(traj, traj_param.delta_arc_length_for_trajectory); + + const auto interpolated_poses_with_yaw = + points_utils::convertToPosesWithYawEstimation(interpolated_points); + const auto opt_nearest_idx = tier4_autoware_utils::findNearestIndex( + interpolated_poses_with_yaw, ego_pose, traj_param.delta_dist_threshold_for_closest_point, + traj_param.delta_yaw_threshold_for_closest_point); + + if (!opt_nearest_idx) { + return false; + } + return true; +} + +std::tuple> calcVehicleCirclesInfo( + const VehicleParam & vehicle_param, const size_t circle_num_for_constraints, + const size_t circle_num_for_radius, const double radius_ratio) +{ + const double radius = std::hypot( + vehicle_param.length / static_cast(circle_num_for_radius) / 2.0, + vehicle_param.width / 2.0) * + radius_ratio; + + std::vector longitudinal_offsets; + const double unit_lon_length = vehicle_param.length / static_cast(circle_num_for_radius); + for (size_t i = 0; i < circle_num_for_constraints; ++i) { + longitudinal_offsets.push_back( + unit_lon_length / 2.0 + + (unit_lon_length * (circle_num_for_radius - 1)) / + static_cast(circle_num_for_constraints - 1) * i - + vehicle_param.rear_overhang); + } + + return {radius, longitudinal_offsets}; +} + +[[maybe_unused]] void fillYawInTrajectoryPoint( + std::vector & traj_points) +{ + std::vector points; + for (const auto & traj_point : traj_points) { + points.push_back(traj_point.pose.position); + } + const auto yaw_vec = interpolation::slerpYawFromPoints(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)); + } +} + +} // namespace + ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) -: Node("obstacle_avoidance_planner", node_options), min_num_points_for_getting_yaw_(2) +: Node("obstacle_avoidance_planner", node_options), + logger_ros_clock_(RCL_ROS_TIME), + eb_solved_count_(0) { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); - tf_buffer_ptr_ = std::make_unique(clock); - tf_listener_ptr_ = std::make_unique(*tf_buffer_ptr_); + // qos rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); - trajectory_pub_ = - create_publisher("~/output/path", 1); - avoiding_traj_pub_ = create_publisher( - "/planning/scenario_planning/lane_driving/obstacle_avoidance_candidate_trajectory", - durable_qos); - debug_smoothed_points_pub_ = create_publisher( - "~/debug/smoothed_points", durable_qos); - is_avoidance_possible_pub_ = create_publisher( - "/planning/scenario_planning/lane_driving/obstacle_avoidance_ready", durable_qos); + // publisher to other nodes + traj_pub_ = create_publisher("~/output/path", 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_clearance_map_pub_ = @@ -75,7 +213,10 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n 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 path_sub_ = create_subscription( "~/input/path", rclcpp::QoS{1}, std::bind(&ObstacleAvoidancePlanner::pathCallback, this, std::placeholders::_1)); @@ -89,213 +230,576 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n "/planning/scenario_planning/lane_driving/obstacle_avoidance_approval", rclcpp::QoS{10}, std::bind(&ObstacleAvoidancePlanner::enableAvoidanceCallback, this, std::placeholders::_1)); - is_publishing_area_with_objects_ = declare_parameter("is_publishing_area_with_objects", false); - is_publishing_clearance_map_ = declare_parameter("is_publishing_clearance_map", false); - is_showing_debug_info_ = declare_parameter("is_showing_debug_info", false); - is_using_vehicle_config_ = declare_parameter("is_using_vehicle_config", false); - is_stopping_if_outside_drivable_area_ = - declare_parameter("is_stopping_if_outside_drivable_area", true); - enable_avoidance_ = declare_parameter("enable_avoidance", true); - - qp_param_ = std::make_unique(); - traj_param_ = std::make_unique(); - constrain_param_ = std::make_unique(); - vehicle_param_ = std::make_unique(); - mpt_param_ = std::make_unique(); - qp_param_->max_iteration = declare_parameter("qp_max_iteration", 10000); - qp_param_->eps_abs = declare_parameter("qp_eps_abs", 1.0e-8); - qp_param_->eps_rel = declare_parameter("qp_eps_rel", 1.0e-11); - qp_param_->eps_abs_for_extending = declare_parameter("qp_eps_abs_for_extending", 1.0e-6); - qp_param_->eps_rel_for_extending = declare_parameter("qp_eps_rel_for_extending", 1.0e-8); - qp_param_->eps_abs_for_visualizing = declare_parameter("qp_eps_abs_for_visualizing", 1.0e-6); - qp_param_->eps_rel_for_visualizing = declare_parameter("qp_eps_rel_for_visualizing", 1.0e-8); - - traj_param_->num_sampling_points = declare_parameter("num_sampling_points", 100); - traj_param_->num_joint_buffer_points = declare_parameter("num_joint_buffer_points", 2); - traj_param_->num_joint_buffer_points_for_extending = - declare_parameter("num_joint_buffer_points_for_extending", 4); - traj_param_->num_offset_for_begin_idx = declare_parameter("num_offset_for_begin_idx", 2); - traj_param_->num_fix_points_for_extending = declare_parameter("num_fix_points_for_extending", 2); - traj_param_->forward_fixing_mpt_distance = declare_parameter("forward_fixing_mpt_distance", 10); - traj_param_->delta_arc_length_for_optimization = - declare_parameter("delta_arc_length_for_optimization", 1.0); - traj_param_->delta_arc_length_for_mpt_points = - declare_parameter("delta_arc_length_for_mpt_points", 1.0); - traj_param_->delta_arc_length_for_trajectory = - declare_parameter("delta_arc_length_for_trajectory", 0.1); - traj_param_->delta_dist_threshold_for_closest_point = - declare_parameter("delta_dist_threshold_for_closest_point", 3.0); - traj_param_->delta_yaw_threshold_for_closest_point = - declare_parameter("delta_yaw_threshold_for_closest_point", 1.0); - traj_param_->delta_yaw_threshold_for_straight = - declare_parameter("delta_yaw_threshold_for_straight", 0.02); - traj_param_->trajectory_length = declare_parameter("trajectory_length", 200); - traj_param_->forward_fixing_distance = declare_parameter("forward_fixing_distance", 10.0); - traj_param_->backward_fixing_distance = declare_parameter("backward_fixing_distance", 5.0); - traj_param_->max_avoiding_ego_velocity_ms = - declare_parameter("max_avoiding_ego_velocity_ms", 6.0); - traj_param_->max_avoiding_objects_velocity_ms = - declare_parameter("max_avoiding_objects_velocity_ms", 0.1); - traj_param_->center_line_width = declare_parameter("center_line_width", 1.7); - traj_param_->acceleration_for_non_deceleration_range = - declare_parameter("acceleration_for_non_deceleration_range", 1.0); - traj_param_->max_dist_for_extending_end_point = - declare_parameter("max_dist_for_extending_end_point", 5.0); - traj_param_->is_avoiding_unknown = declare_parameter("avoiding_object_type.unknown", true); - traj_param_->is_avoiding_car = declare_parameter("avoiding_object_type.car", true); - traj_param_->is_avoiding_truck = declare_parameter("avoiding_object_type.truck", true); - traj_param_->is_avoiding_bus = declare_parameter("avoiding_object_type.bus", true); - traj_param_->is_avoiding_bicycle = declare_parameter("avoiding_object_type.bicycle", true); - traj_param_->is_avoiding_motorbike = declare_parameter("avoiding_object_type.motorbike", true); - traj_param_->is_avoiding_pedestrian = declare_parameter("avoiding_object_type.pedestrian", true); - traj_param_->is_avoiding_animal = declare_parameter("avoiding_object_type.animal", true); - - constrain_param_->is_getting_constraints_close2path_points = - declare_parameter("is_getting_constraints_close2path_points", false); - constrain_param_->clearance_for_straight_line = - declare_parameter("clearance_for_straight_line", 0.05); - constrain_param_->clearance_for_joint = declare_parameter("clearance_for_joint", 0.1); - constrain_param_->range_for_extend_joint = declare_parameter("range_for_extend_joint", 1.6); - constrain_param_->clearance_for_only_smoothing = - declare_parameter("clearance_for_only_smoothing", 0.1); - constrain_param_->clearance_from_object_for_straight = - declare_parameter("clearance_from_object_for_straight", 10.0); - constrain_param_->clearance_from_road = declare_parameter("clearance_from_road", 0.1); - constrain_param_->clearance_from_object = declare_parameter("clearance_from_object", 0.6); - constrain_param_->extra_desired_clearance_from_road = - declare_parameter("extra_desired_clearance_from_road", 0.2); - constrain_param_->min_object_clearance_for_joint = - declare_parameter("min_object_clearance_for_joint", 3.2); - constrain_param_->max_x_constrain_search_range = - declare_parameter("max_x_constrain_search_range", 0.4); - constrain_param_->coef_x_constrain_search_resolution = - declare_parameter("coef_x_constrain_search_resolution", 1.0); - constrain_param_->coef_y_constrain_search_resolution = - declare_parameter("coef_y_constrain_search_resolution", 0.5); - constrain_param_->keep_space_shape_x = declare_parameter("keep_space_shape_x", 3.0); - constrain_param_->keep_space_shape_y = declare_parameter("keep_space_shape_y", 2.0); - constrain_param_->max_lon_space_for_driveable_constraint = - declare_parameter("max_lon_space_for_driveable_constraint", 0.5); - constrain_param_->clearance_for_fixing = 0.0; - - min_delta_dist_for_replan_ = declare_parameter("min_delta_dist_for_replan", 5.0); - min_delta_time_sec_for_replan_ = declare_parameter("min_delta_time_sec_for_replan", 1.0); - distance_for_path_shape_change_detection_ = - declare_parameter("distance_for_path_shape_change_detection", 2.0); - - // vehicle param - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - 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; - - if (is_using_vehicle_config_) { - double vehicle_width = vehicle_info.vehicle_width_m; - traj_param_->center_line_width = vehicle_width; - constrain_param_->keep_space_shape_y = vehicle_width; - } - constrain_param_->min_object_clearance_for_deceleration = - constrain_param_->clearance_from_object + constrain_param_->keep_space_shape_y * 0.5; - - double max_steer_deg = 0; - max_steer_deg = declare_parameter("max_steer_deg", 30.0); - vehicle_param_->max_steer_rad = max_steer_deg * M_PI / 180.0; - vehicle_param_->steer_tau = declare_parameter("steer_tau", 0.1); - - // mpt param - mpt_param_->is_hard_fixing_terminal_point = - declare_parameter("is_hard_fixing_terminal_point", true); - mpt_param_->num_curvature_sampling_points = declare_parameter("num_curvature_sampling_points", 5); - mpt_param_->base_point_weight = declare_parameter("base_point_weight", 2000.0); - mpt_param_->top_point_weight = declare_parameter("top_point_weight", 1000.0); - mpt_param_->mid_point_weight = declare_parameter("mid_point_weight", 1000.0); - mpt_param_->lat_error_weight = declare_parameter("lat_error_weight", 10.0); - mpt_param_->yaw_error_weight = declare_parameter("yaw_error_weight", 0.0); - mpt_param_->steer_input_weight = declare_parameter("steer_input_weight", 0.1); - mpt_param_->steer_rate_weight = declare_parameter("steer_rate_weight", 100.0); - mpt_param_->steer_acc_weight = declare_parameter("steer_acc_weight", 0.000001); - mpt_param_->terminal_lat_error_weight = declare_parameter("terminal_lat_error_weight", 0.0); - mpt_param_->terminal_yaw_error_weight = declare_parameter("terminal_yaw_error_weight", 100.0); - mpt_param_->terminal_path_lat_error_weight = - declare_parameter("terminal_path_lat_error_weight", 1000.0); - mpt_param_->terminal_path_yaw_error_weight = - declare_parameter("terminal_path_yaw_error_weight", 1000.0); - mpt_param_->zero_ff_steer_angle = declare_parameter("zero_ff_steer_angle", 0.5); - - mpt_param_->clearance_from_road = vehicle_param_->width * 0.5 + - constrain_param_->clearance_from_road + - constrain_param_->extra_desired_clearance_from_road; - mpt_param_->clearance_from_object = - vehicle_param_->width * 0.5 + constrain_param_->clearance_from_object; - mpt_param_->base_point_dist_from_base_link = 0; - mpt_param_->top_point_dist_from_base_link = - (vehicle_param_->length - vehicle_param_->rear_overhang); - mpt_param_->mid_point_dist_from_base_link = - (mpt_param_->base_point_dist_from_base_link + mpt_param_->top_point_dist_from_base_link) * 0.5; - - in_objects_ptr_ = std::make_unique(); + { // vehicle param + vehicle_param_ = VehicleParam{}; + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + 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; + } + + { // 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"); + + // drivability check + use_vehicle_circles_for_drivability_ = + declare_parameter("advanced.option.drivability_check.use_vehicle_circles"); + if (use_vehicle_circles_for_drivability_) { + // vehicle_circles + // NOTE: Vehicle shape for drivability check is considered as a set of circles + use_manual_vehicle_circles_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.use_manual_vehicle_circles"); + vehicle_circle_constraints_num_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.num_for_constraints"); + if (use_manual_vehicle_circles_for_drivability_) { // vehicle circles are designated manually + vehicle_circle_longitudinal_offsets_for_drivability_ = + declare_parameter>( + "advanced.option.drivability_check.vehicle_circles.longitudinal_offsets"); + vehicle_circle_radius_for_drivability_ = + declare_parameter("advanced.option.drivability_check.vehicle_circles.radius"); + } else { // vehicle circles are calculated automatically with designated ratio + const int default_radius_num = + std::round(vehicle_param_.length / vehicle_param_.width * 1.5); + + vehicle_circle_radius_num_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.num_for_radius", default_radius_num); + vehicle_circle_radius_ratio_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.radius_ratio"); + + std::tie( + vehicle_circle_radius_for_drivability_, + vehicle_circle_longitudinal_offsets_for_drivability_) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_drivability_, + vehicle_circle_radius_num_for_drivability_, + vehicle_circle_radius_ratio_for_drivability_); + } + } + + 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"); + } + + { // 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"); + + // 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); + } + + { // 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 = false; + // mpt_param_.plan_from_ego = declare_parameter("mpt.option.plan_from_ego"); + 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"); + + // 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 = + declare_parameter("mpt.kinematics.max_steer_deg") * M_PI / 180.0; + + // 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); + + // 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 + use_manual_vehicle_circles_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.use_manual_vehicle_circles"); + vehicle_circle_constraints_num_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_constraints"); + if (use_manual_vehicle_circles_for_mpt_) { // vehicle circles are designated manually + mpt_param_.vehicle_circle_longitudinal_offsets = declare_parameter>( + "advanced.mpt.collision_free_constraints.vehicle_circles.longitudinal_offsets"); + mpt_param_.vehicle_circle_radius = + declare_parameter("advanced.mpt.collision_free_constraints.vehicle_circles.radius"); + } else { // vehicle circles are calculated automatically with designated ratio + const int default_radius_num = std::round(vehicle_param_.length / vehicle_param_.width * 1.5); + + vehicle_circle_radius_num_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_radius", + default_radius_num); + vehicle_circle_radius_ratio_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.radius_ratio"); + + std::tie(mpt_param_.vehicle_circle_radius, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_mpt_, + vehicle_circle_radius_num_for_mpt_, vehicle_circle_radius_ratio_for_mpt_); + } + + // 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"); + } + + { // 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; + + objects_ptr_ = std::make_unique(); // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleAvoidancePlanner::paramCallback, this, std::placeholders::_1)); - initialize(); -} + resetPlanning(); -ObstacleAvoidancePlanner::~ObstacleAvoidancePlanner() {} + self_pose_listener_.waitForFirstPose(); +} rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback( const std::vector & parameters) { - auto update_param = [&](const std::string & name, double & v) { - auto it = std::find_if( - parameters.cbegin(), parameters.cend(), - [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); - if (it != parameters.cend()) { - v = it->as_double(); - return true; + 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_); + + // drivability check + updateParam( + parameters, "advanced.option.drivability_check.use_vehicle_circles", + use_vehicle_circles_for_drivability_); + if (use_vehicle_circles_for_drivability_) { + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.use_manual_vehicle_circles", + use_manual_vehicle_circles_for_drivability_); + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.num_for_constraints", + vehicle_circle_constraints_num_for_drivability_); + if (use_manual_vehicle_circles_for_drivability_) { + updateParam>( + parameters, "advanced.option.drivability_check.vehicle_circles.longitudinal_offsets", + vehicle_circle_longitudinal_offsets_for_drivability_); + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.radius", + vehicle_circle_radius_for_drivability_); + } else { + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.num_for_radius", + vehicle_circle_radius_num_for_drivability_); + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.radius_ratio", + vehicle_circle_radius_ratio_for_drivability_); + + std::tie( + vehicle_circle_radius_for_drivability_, + vehicle_circle_longitudinal_offsets_for_drivability_) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_drivability_, + vehicle_circle_radius_num_for_drivability_, + vehicle_circle_radius_ratio_for_drivability_); + } } - return false; - }; - - // trajectory total/fixing length - update_param("trajectory_length", traj_param_->trajectory_length); - update_param("forward_fixing_distance", traj_param_->forward_fixing_distance); - update_param("backward_fixing_distance", traj_param_->backward_fixing_distance); - - // clearance for unique points - update_param("clearance_for_straight_line", constrain_param_->clearance_for_straight_line); - update_param("clearance_for_joint", constrain_param_->clearance_for_joint); - update_param("clearance_for_only_smoothing", constrain_param_->clearance_for_only_smoothing); - update_param( - "clearance_from_object_for_straight", constrain_param_->clearance_from_object_for_straight); - - // clearance(distance) when generating trajectory - update_param("clearance_from_road", constrain_param_->clearance_from_road); - update_param("clearance_from_object", constrain_param_->clearance_from_object); - update_param("min_object_clearance_for_joint", constrain_param_->min_object_clearance_for_joint); - update_param( - "extra_desired_clearance_from_road", constrain_param_->extra_desired_clearance_from_road); - - // avoiding param - update_param("max_avoiding_objects_velocity_ms", traj_param_->max_avoiding_objects_velocity_ms); - update_param("max_avoiding_ego_velocity_ms", traj_param_->max_avoiding_ego_velocity_ms); - update_param("center_line_width", traj_param_->center_line_width); - update_param( - "acceleration_for_non_deceleration_range", - traj_param_->acceleration_for_non_deceleration_range); - - // mpt param - update_param("base_point_weight", mpt_param_->base_point_weight); - update_param("top_point_weight", mpt_param_->top_point_weight); - update_param("mid_point_weight", mpt_param_->mid_point_weight); - update_param("lat_error_weight", mpt_param_->lat_error_weight); - update_param("yaw_error_weight", mpt_param_->yaw_error_weight); - update_param("steer_input_weight", mpt_param_->steer_input_weight); - update_param("steer_rate_weight", mpt_param_->steer_rate_weight); - update_param("steer_acc_weight", mpt_param_->steer_acc_weight); + + 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_); + } + + { // 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); + } + + { // 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); + } + + { // mpt param + // option + // updateParam(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego); + 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_); + + // 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 + double max_steer_deg = mpt_param_.max_steer_rad * 180.0 / M_PI; + updateParam(parameters, "mpt.kinematics.max_steer_deg", max_steer_deg); + mpt_param_.max_steer_rad = max_steer_deg * M_PI / 180.0; + 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 + updateParam( + parameters, + "advanced.mpt.collision_free_constraints.vehicle_circles.use_manual_vehicle_circles", + use_manual_vehicle_circles_for_mpt_); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_constraints", + vehicle_circle_constraints_num_for_mpt_); + if (use_manual_vehicle_circles_for_mpt_) { + updateParam>( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.longitudinal_offsets", + mpt_param_.vehicle_circle_longitudinal_offsets); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.radius", + mpt_param_.vehicle_circle_radius); + } else { // vehicle circles are calculated automatically with designated ratio + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_radius", + vehicle_circle_radius_num_for_mpt_); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.radius_ratio", + vehicle_circle_radius_ratio_for_mpt_); + + std::tie(mpt_param_.vehicle_circle_radius, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_mpt_, + vehicle_circle_radius_num_for_mpt_, vehicle_circle_radius_ratio_for_mpt_); + } + + // 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); + } + + { // 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_); + } + + resetPlanning(); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -303,21 +807,6 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback return result; } -// ROS callback functions -void ObstacleAvoidancePlanner::pathCallback( - const autoware_auto_planning_msgs::msg::Path::SharedPtr msg) -{ - std::lock_guard lock(mutex_); - current_ego_pose_ptr_ = getCurrentEgoPose(); - if ( - msg->points.empty() || msg->drivable_area.data.empty() || !current_ego_pose_ptr_ || - !current_twist_ptr_) { - return; - } - autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg = generateTrajectory(*msg); - trajectory_pub_->publish(output_trajectory_msg); -} - void ObstacleAvoidancePlanner::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_twist_ptr_ = std::make_unique(); @@ -328,7 +817,7 @@ void ObstacleAvoidancePlanner::odomCallback(const nav_msgs::msg::Odometry::Share void ObstacleAvoidancePlanner::objectsCallback( const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) { - in_objects_ptr_ = std::make_unique(*msg); + objects_ptr_ = std::make_unique(*msg); } void ObstacleAvoidancePlanner::enableAvoidanceCallback( @@ -336,508 +825,617 @@ void ObstacleAvoidancePlanner::enableAvoidanceCallback( { enable_avoidance_ = msg->enable_avoidance; } -// End ROS callback functions -autoware_auto_planning_msgs::msg::Trajectory ObstacleAvoidancePlanner::generateTrajectory( - const autoware_auto_planning_msgs::msg::Path & path) +void ObstacleAvoidancePlanner::resetPlanning() { - auto t_start = std::chrono::high_resolution_clock::now(); + RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - const auto traj_points = generateOptimizedTrajectory(*current_ego_pose_ptr_, path); + costmap_generator_ptr_ = std::make_unique(); - const auto post_processed_traj = - generatePostProcessedTrajectory(*current_ego_pose_ptr_, path.points, traj_points); + eb_path_optimizer_ptr_ = std::make_unique( + is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); - auto output = tier4_autoware_utils::convertToTrajectory(post_processed_traj); - output.header = path.header; + mpt_optimizer_ptr_ = + std::make_unique(is_showing_debug_info_, traj_param_, vehicle_param_, mpt_param_); - prev_path_points_ptr_ = - std::make_unique>(path.points); - - auto t_end = std::chrono::high_resolution_clock::now(); - float elapsed_ms = std::chrono::duration(t_end - t_start).count(); - RCLCPP_INFO_EXPRESSION( - get_logger(), is_showing_debug_info_, - "Total time: = %f [ms]\n==========================", elapsed_ms); - return output; + prev_path_points_ptr_ = nullptr; + resetPrevOptimization(); } -std::vector -ObstacleAvoidancePlanner::generateOptimizedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path) +void ObstacleAvoidancePlanner::resetPrevOptimization() { - if (!needReplan( - ego_pose, prev_ego_pose_ptr_, path.points, prev_replanned_time_ptr_, prev_path_points_ptr_, - prev_trajectories_ptr_)) { - return getPrevTrajectory(path.points); - } - prev_ego_pose_ptr_ = std::make_unique(ego_pose); - prev_replanned_time_ptr_ = std::make_unique(this->now()); - - DebugData debug_data; - const auto optional_trajs = eb_path_optimizer_ptr_->generateOptimizedTrajectory( - enable_avoidance_, ego_pose, path, prev_trajectories_ptr_, in_objects_ptr_->objects, - &debug_data); - if (!optional_trajs) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "[Avoidance] Optimization failed, passing previous trajectory"); - const bool is_prev_traj = true; - const auto prev_trajs_inside_area = calcTrajectoryInsideArea( - getPrevTrajs(path.points), path.points, debug_data.clearance_map, path.drivable_area.info, - &debug_data, is_prev_traj); - prev_trajectories_ptr_ = std::make_unique( - makePrevTrajectories(*current_ego_pose_ptr_, path.points, prev_trajs_inside_area.get())); - - const auto prev_traj = util::concatTraj(prev_trajs_inside_area.get()); - publishingDebugData(debug_data, path, prev_traj, *vehicle_param_); - return prev_traj; - } - - const auto trajs_inside_area = getTrajectoryInsideArea( - optional_trajs.get(), path.points, debug_data.clearance_map, path.drivable_area.info, - &debug_data); - - prev_trajectories_ptr_ = std::make_unique( - makePrevTrajectories(*current_ego_pose_ptr_, path.points, trajs_inside_area)); - const auto optimized_trajectory = util::concatTraj(trajs_inside_area); - publishingDebugData(debug_data, path, optimized_trajectory, *vehicle_param_); - return optimized_trajectory; + prev_optimal_trajs_ptr_ = nullptr; + eb_solved_count_ = 0; } -void ObstacleAvoidancePlanner::initialize() +void ObstacleAvoidancePlanner::pathCallback( + const autoware_auto_planning_msgs::msg::Path::SharedPtr path_ptr) { - RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Resetting"); - eb_path_optimizer_ptr_ = std::make_unique( - is_showing_debug_info_, *qp_param_, *traj_param_, *constrain_param_, *vehicle_param_, - *mpt_param_); - prev_path_points_ptr_ = nullptr; - prev_trajectories_ptr_ = nullptr; -} + stop_watch_.tic(__func__); -std::unique_ptr ObstacleAvoidancePlanner::getCurrentEgoPose() -{ - geometry_msgs::msg::TransformStamped tf_current_pose; - - try { - tf_current_pose = tf_buffer_ptr_->lookupTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(get_logger(), "[ObstacleAvoidancePlanner] %s", ex.what()); - return nullptr; - } - - geometry_msgs::msg::Pose p; - p.orientation = tf_current_pose.transform.rotation; - p.position.x = tf_current_pose.transform.translation.x; - p.position.y = tf_current_pose.transform.translation.y; - p.position.z = tf_current_pose.transform.translation.z; - std::unique_ptr p_ptr = std::make_unique(p); - return p_ptr; + if (path_ptr->points.empty() || path_ptr->drivable_area.data.empty() || !current_twist_ptr_) { + return; + } + + current_ego_pose_ = self_pose_listener_.getCurrentPose()->pose; + debug_data_ptr_ = std::make_shared(); + debug_data_ptr_->init( + is_showing_calculation_time_, mpt_visualize_sampling_num_, current_ego_pose_, + mpt_param_.vehicle_circle_radius, mpt_param_.vehicle_circle_longitudinal_offsets); + + // generate optimized trajectory + const auto optimized_traj_points = generateOptimizedTrajectory(*path_ptr); + + // generate post processed trajectory + const auto post_processed_traj_points = + generatePostProcessedTrajectory(path_ptr->points, optimized_traj_points); + + // convert to output msg type + auto output_traj_msg = tier4_autoware_utils::convertToTrajectory(post_processed_traj_points); + output_traj_msg.header = path_ptr->header; + + // publish debug data + publishDebugDataInMain(*path_ptr); + + { // print and publish debug msg + debug_data_ptr_->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_ptr_->msg_stream.getString(); + debug_msg_pub_->publish(debug_msg_msg); + } + + // make previous variables + prev_path_points_ptr_ = + std::make_unique>(path_ptr->points); + prev_ego_pose_ptr_ = std::make_unique(current_ego_pose_); + + traj_pub_->publish(output_traj_msg); } std::vector -ObstacleAvoidancePlanner::generatePostProcessedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::vector & optimized_points) const +ObstacleAvoidancePlanner::generateOptimizedTrajectory( + const autoware_auto_planning_msgs::msg::Path & path) { - auto t_start = std::chrono::high_resolution_clock::now(); + stop_watch_.tic(__func__); - std::vector trajectory_points; - if (path_points.empty()) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; - tmp_point.pose = ego_pose; - tmp_point.longitudinal_velocity_mps = 0; - trajectory_points.push_back(tmp_point); - return trajectory_points; + if (reset_prev_optimization_) { + resetPrevOptimization(); } - if (optimized_points.empty()) { - trajectory_points = util::convertPathToTrajectory(path_points); - return trajectory_points; + + // return prev trajectory if replan is not required + if (!checkReplan(path.points)) { + if (prev_optimal_trajs_ptr_) { + return prev_optimal_trajs_ptr_->model_predictive_trajectory; + } + + return points_utils::convertToTrajectoryPoints(path.points); } - trajectory_points = convertPointsToTrajectory(path_points, optimized_points); + latest_replanned_time_ptr_ = std::make_unique(this->now()); + + // create clearance maps + const CVMaps cv_maps = costmap_generator_ptr_->getMaps( + enable_avoidance_, path, objects_ptr_->objects, traj_param_, debug_data_ptr_); + + // calculate trajectory with EB and MPT + auto optimal_trajs = optimizeTrajectory(path, cv_maps); + + // insert 0 velocity when trajectory is over drivable area + if (is_stopping_if_outside_drivable_area_) { + insertZeroVelocityOutsideDrivableArea(optimal_trajs.model_predictive_trajectory, cv_maps); + } + + publishDebugDataInOptimization(path, optimal_trajs.model_predictive_trajectory); - auto t_end = std::chrono::high_resolution_clock::now(); - float elapsed_ms = std::chrono::duration(t_end - t_start).count(); - RCLCPP_INFO_EXPRESSION( - get_logger(), is_showing_debug_info_, "Post processing time: = %f [ms]", elapsed_ms); + // make previous trajectories + prev_optimal_trajs_ptr_ = + std::make_unique(makePrevTrajectories(path.points, optimal_trajs)); - return trajectory_points; + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return optimal_trajs.model_predictive_trajectory; } -bool ObstacleAvoidancePlanner::needReplan( - const geometry_msgs::msg::Pose & ego_pose, - const std::unique_ptr & prev_ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_replanned_time, - const std::unique_ptr> & - prev_path_points, - std::unique_ptr & prev_trajs) +// check if optimization is required or not. +// NOTE: previous trajectories information will be reset as well in some cases. +bool ObstacleAvoidancePlanner::checkReplan( + const std::vector & path_points) { - if (!prev_ego_pose || !prev_replanned_time || !prev_path_points || !prev_trajs) { + if ( + !prev_ego_pose_ptr_ || !latest_replanned_time_ptr_ || !prev_path_points_ptr_ || + !prev_optimal_trajs_ptr_) { + return true; + } + + const double max_mpt_length = + traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; + if (isPathShapeChanged( + current_ego_pose_, path_points, prev_path_points_ptr_, max_mpt_length, + max_path_shape_change_dist_for_replan_, + traj_param_.delta_yaw_threshold_for_closest_point)) { + RCLCPP_INFO(get_logger(), "Replan since path shape was changed."); return true; } - if (isPathShapeChanged(ego_pose, path_points, prev_path_points)) { - RCLCPP_INFO(get_logger(), "[Avoidance] Path shape is changed, reset prev trajs"); - prev_trajs = nullptr; + if (isPathGoalChanged(current_twist_ptr_->twist.linear.x, path_points, prev_path_points_ptr_)) { + RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed."); + resetPrevOptimization(); return true; } - if (!util::hasValidNearestPointFromEgo( - *current_ego_pose_ptr_, *prev_trajectories_ptr_, *traj_param_)) { + // For when ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(current_ego_pose_.position, prev_ego_pose_ptr_->position); + if (delta_dist > max_ego_moving_dist_for_replan_) { RCLCPP_INFO( - get_logger(), "[Avoidance] Could not find valid nearest point from ego, reset prev trajs"); - prev_trajs = nullptr; + get_logger(), + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + resetPrevOptimization(); return true; } - const double delta_dist = util::calculate2DDistance(ego_pose.position, prev_ego_pose->position); - if (delta_dist > min_delta_dist_for_replan_) { + // For when ego pose moves far from trajectory + if (!hasValidNearestPointFromEgo(current_ego_pose_, *prev_optimal_trajs_ptr_, traj_param_)) { + RCLCPP_INFO( + get_logger(), + "Replan with resetting optimization since valid nearest trajectory point from ego was not " + "found."); + resetPrevOptimization(); return true; } - rclcpp::Duration delta_time = this->now() - *prev_replanned_time; - const double delta_time_sec = delta_time.seconds(); - if (delta_time_sec > min_delta_time_sec_for_replan_) { + const double delta_time_sec = (this->now() - *latest_replanned_time_ptr_).seconds(); + if (delta_time_sec > max_delta_time_sec_for_replan_) { return true; } return false; } -bool ObstacleAvoidancePlanner::isPathShapeChanged( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr> & - prev_path_points) +Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( + const autoware_auto_planning_msgs::msg::Path & path, const CVMaps & cv_maps) { - if (!prev_path_points) { - return true; + stop_watch_.tic(__func__); + + if (skip_optimization_) { + const auto traj = points_utils::convertToTrajectoryPoints(path.points); + Trajectories trajs; + trajs.smoothed_trajectory = traj; + trajs.model_predictive_trajectory = traj; + return trajs; } - const int default_nearest_prev_path_idx = 0; - const int nearest_prev_path_idx = util::getNearestIdx( - *prev_path_points, ego_pose, default_nearest_prev_path_idx, - traj_param_->delta_yaw_threshold_for_closest_point); - const int default_nearest_path_idx = 0; - const int nearest_path_idx = util::getNearestIdx( - path_points, ego_pose, default_nearest_path_idx, - traj_param_->delta_yaw_threshold_for_closest_point); - const auto prev_first = prev_path_points->begin() + nearest_prev_path_idx; - const auto prev_last = prev_path_points->end(); - std::vector truncated_prev_points( - prev_first, prev_last); + // 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( + current_ego_pose_, path, prev_optimal_trajs_ptr_, current_twist_ptr_->twist.linear.x, + debug_data_ptr_); + } + return points_utils::convertToTrajectoryPoints(path.points); + }(); + if (!eb_traj) { + return getPrevTrajs(path.points); + } - const auto first = path_points.begin() + nearest_path_idx; - const auto last = path_points.end(); - std::vector truncated_points(first, last); + // 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_++; - for (const auto & prev_point : truncated_prev_points) { - double min_dist = std::numeric_limits::max(); - for (const auto & point : truncated_points) { - const double dist = util::calculate2DDistance(point.pose.position, prev_point.pose.position); - if (dist < min_dist) { - min_dist = dist; - } - } - if (min_dist > distance_for_path_shape_change_detection_) { - return true; + if (prev_optimal_trajs_ptr_) { + prev_optimal_trajs_ptr_->model_predictive_trajectory.clear(); + prev_optimal_trajs_ptr_->mpt_ref_points.clear(); } } - return false; -} - -std::vector -ObstacleAvoidancePlanner::convertPointsToTrajectory( - const std::vector & path_points, - const std::vector & trajectory_points) const -{ - std::vector interpolated_points = - util::getInterpolatedPoints(trajectory_points, traj_param_->delta_arc_length_for_trajectory); - // add discarded point in the process of interpolation - interpolated_points.push_back(trajectory_points.back().pose.position); - if (static_cast(interpolated_points.size()) < min_num_points_for_getting_yaw_) { - return util::convertPathToTrajectory(path_points); - } - std::vector candidate_points = interpolated_points; - geometry_msgs::msg::Pose last_pose; - last_pose.position = candidate_points.back(); - last_pose.orientation = util::getQuaternionFromPoints( - candidate_points.back(), candidate_points[candidate_points.size() - 2]); - const auto extended_point_opt = util::getLastExtendedPoint( - path_points.back(), last_pose, traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_point_opt) { - candidate_points.push_back(extended_point_opt.get()); - } - - const int zero_velocity_idx = util::getZeroVelocityIdx( - is_showing_debug_info_, candidate_points, path_points, prev_trajectories_ptr_, *traj_param_); - - auto traj_points = util::convertPointsToTrajectoryPointsWithYaw(candidate_points); - traj_points = util::fillTrajectoryWithVelocity(traj_points, 1e4); - if (prev_trajectories_ptr_) { - const int max_skip_comparison_velocity_idx_for_optimized_points = - calculateNonDecelerationRange(traj_points, *current_ego_pose_ptr_, current_twist_ptr_->twist); - const auto optimized_trajectory = util::concatTraj(*prev_trajectories_ptr_); - traj_points = util::alignVelocityWithPoints( - traj_points, optimized_trajectory, zero_velocity_idx, - max_skip_comparison_velocity_idx_for_optimized_points); - } - const int max_skip_comparison_idx_for_path_points = -1; - traj_points = util::alignVelocityWithPoints( - traj_points, path_points, zero_velocity_idx, max_skip_comparison_idx_for_path_points); - - return traj_points; -} -void ObstacleAvoidancePlanner::publishingDebugData( - const DebugData & debug_data, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & traj_points, - const VehicleParam & vehicle_param) -{ - auto traj = tier4_autoware_utils::convertToTrajectory(debug_data.foa_data.avoiding_traj_points); - traj.header = path.header; - avoiding_traj_pub_->publish(traj); - - auto debug_smoothed_points = - tier4_autoware_utils::convertToTrajectory(debug_data.smoothed_points); - debug_smoothed_points.header = path.header; - debug_smoothed_points_pub_->publish(debug_smoothed_points); - - tier4_planning_msgs::msg::IsAvoidancePossible is_avoidance_possible; - is_avoidance_possible.is_avoidance_possible = debug_data.foa_data.is_avoidance_possible; - is_avoidance_possible_pub_->publish(is_avoidance_possible); - - std::vector traj_points_debug = traj_points; - // Add z information for virtual wall - if (!traj_points_debug.empty()) { - const int idx = util::getNearestIdx( - path.points, traj_points.back().pose, 0, traj_param_->delta_yaw_threshold_for_closest_point); - traj_points_debug.back().pose.position.z = path.points.at(idx).pose.position.z + 1.0; - } - - debug_markers_pub_->publish( - getDebugVisualizationMarker(debug_data, traj_points_debug, vehicle_param)); - if (is_publishing_area_with_objects_) { - debug_area_with_objects_pub_->publish( - getDebugCostmap(debug_data.area_with_objects_map, path.drivable_area)); - } - if (is_publishing_clearance_map_) { - debug_clearance_map_pub_->publish( - getDebugCostmap(debug_data.clearance_map, path.drivable_area)); - debug_object_clearance_map_pub_->publish( - getDebugCostmap(debug_data.only_object_clearance_map, path.drivable_area)); + // MPT: optimize trajectory to be kinematically feasible and collision free + const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( + enable_avoidance_, eb_traj.get(), path.points, prev_optimal_trajs_ptr_, cv_maps, + current_ego_pose_, current_twist_ptr_->twist.linear.x, debug_data_ptr_); + if (!mpt_trajs) { + return getPrevTrajs(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_ptr_->mpt_traj = mpt_trajs.get().mpt; + debug_data_ptr_->mpt_ref_traj = + points_utils::convertToTrajectoryPoints(mpt_trajs.get().ref_points); + debug_data_ptr_->eb_traj = eb_traj.get(); + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return trajs; } -int ObstacleAvoidancePlanner::calculateNonDecelerationRange( - const std::vector & traj_points, - const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Twist & ego_twist) const +Trajectories ObstacleAvoidancePlanner::getPrevTrajs( + const std::vector & path_points) const { - const int default_idx = 0; - const int nearest_idx = util::getNearestIdx( - traj_points, ego_pose, default_idx, traj_param_->delta_yaw_threshold_for_closest_point); - const double non_decelerating_arc_length = - (ego_twist.linear.x - traj_param_->max_avoiding_ego_velocity_ms) / - traj_param_->acceleration_for_non_deceleration_range; - if (non_decelerating_arc_length < 0 || traj_points.size() < 2) { - return 0; - } - - double accum_arc_length = 0; - for (std::size_t i = nearest_idx + 1; i < traj_points.size(); i++) { - accum_arc_length += - util::calculate2DDistance(traj_points[i].pose.position, traj_points[i - 1].pose.position); - if (accum_arc_length > non_decelerating_arc_length) { - return i; - } + if (prev_optimal_trajs_ptr_) { + return *prev_optimal_trajs_ptr_; } - return 0; -} -Trajectories ObstacleAvoidancePlanner::getTrajectoryInsideArea( - const Trajectories & trajs, - const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data) const -{ - debug_data->current_vehicle_footprints = - util::getVehicleFootprints(trajs.model_predictive_trajectory, *vehicle_param_); - const auto current_trajs_inside_area = - calcTrajectoryInsideArea(trajs, path_points, road_clearance_map, map_info, debug_data); - if (!current_trajs_inside_area) { - RCLCPP_WARN( - get_logger(), - "[Avoidance] Current trajectory is not inside drivable area, passing previous one. " - "Might stop at the end of drivable trajectory."); - auto prev_trajs = getPrevTrajs(path_points); - const bool is_prev_traj = true; - const auto prev_trajs_inside_area = calcTrajectoryInsideArea( - prev_trajs, path_points, road_clearance_map, map_info, debug_data, is_prev_traj); - return prev_trajs_inside_area.get(); - } - return current_trajs_inside_area.get(); + const auto traj = points_utils::convertToTrajectoryPoints(path_points); + Trajectories trajs; + trajs.smoothed_trajectory = traj; + trajs.model_predictive_trajectory = traj; + return trajs; } -boost::optional ObstacleAvoidancePlanner::calcTrajectoryInsideArea( - const Trajectories & trajs, - const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data, const bool is_prev_traj) const +void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( + std::vector & traj_points, + const CVMaps & cv_maps) { - if (!is_stopping_if_outside_drivable_area_) { - const auto stop_idx = getStopIdx(path_points, trajs, map_info, road_clearance_map, debug_data); - if (stop_idx) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_WARN_THROTTLE( - get_logger(), clock, std::chrono::milliseconds(1000).count(), - "[Avoidance] Expecting over drivable area"); + stop_watch_.tic(__func__); + + const auto & map_info = cv_maps.map_info; + const auto & road_clearance_map = cv_maps.clearance_map; + + const size_t nearest_idx = + tier4_autoware_utils::findNearestIndex(traj_points, current_ego_pose_.position); + + // 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 getBaseTrajectory(path_points, trajs); - } - const auto optional_stop_idx = - getStopIdx(path_points, trajs, map_info, road_clearance_map, debug_data); - if (!is_prev_traj && optional_stop_idx) { - return boost::none; - } - - auto tmp_trajs = getBaseTrajectory(path_points, trajs); - if (is_prev_traj) { - tmp_trajs.extended_trajectory = - std::vector{}; - debug_data->is_expected_to_over_drivable_area = true; - } - - if (optional_stop_idx && !prev_trajectories_ptr_) { - if (optional_stop_idx.get() < static_cast(trajs.model_predictive_trajectory.size())) { - tmp_trajs.model_predictive_trajectory = - std::vector{ - trajs.model_predictive_trajectory.begin(), - trajs.model_predictive_trajectory.begin() + optional_stop_idx.get()}; - tmp_trajs.extended_trajectory = - std::vector{}; - debug_data->is_expected_to_over_drivable_area = true; + return traj_points.size(); + }(); + + 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 = [&]() { + if (use_vehicle_circles_for_drivability_) { + return cv_drivable_area_utils::isOutsideDrivableAreaFromCirclesFootprint( + traj_point, road_clearance_map, map_info, + vehicle_circle_longitudinal_offsets_for_drivability_, + vehicle_circle_radius_for_drivability_); + } + return cv_drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( + traj_point, road_clearance_map, map_info, vehicle_param_); + }(); + + // only insert zero velocity to the first point outside drivable area + if (is_outside) { + traj_points[i].longitudinal_velocity_mps = 0.0; + debug_data_ptr_->stop_pose_by_drivable_area = traj_points[i].pose; + break; } } - return tmp_trajs; + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; } -Trajectories ObstacleAvoidancePlanner::getPrevTrajs( - const std::vector & path_points) const +void ObstacleAvoidancePlanner::publishDebugDataInOptimization( + const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & traj_points) { - if (!prev_trajectories_ptr_) { - const auto traj = util::convertPathToTrajectory(path_points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; - } else { - return *prev_trajectories_ptr_; + stop_watch_.tic(__func__); + + { // publish trajectories + auto debug_eb_traj = tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->eb_traj); + debug_eb_traj.header = path.header; + debug_eb_traj_pub_->publish(debug_eb_traj); + + auto debug_mpt_fixed_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_fixed_traj); + debug_mpt_fixed_traj.header = path.header; + debug_mpt_fixed_traj_pub_->publish(debug_mpt_fixed_traj); + + auto debug_mpt_ref_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_ref_traj); + debug_mpt_ref_traj.header = path.header; + debug_mpt_ref_traj_pub_->publish(debug_mpt_ref_traj); + + auto debug_mpt_traj = tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_traj); + debug_mpt_traj.header = path.header; + debug_mpt_traj_pub_->publish(debug_mpt_traj); } -} -std::vector -ObstacleAvoidancePlanner::getPrevTrajectory( - const std::vector & path_points) const -{ - std::vector traj; - const auto & trajs = getPrevTrajs(path_points); - traj.insert( - traj.end(), trajs.model_predictive_trajectory.begin(), trajs.model_predictive_trajectory.end()); - traj.insert(traj.end(), trajs.extended_trajectory.begin(), trajs.extended_trajectory.end()); - return traj; + { // publish markers + if (is_publishing_debug_visualization_marker_) { + stop_watch_.tic("getDebugVisualizationMarker"); + const auto & debug_marker = debug_visualization::getDebugVisualizationMarker( + debug_data_ptr_, traj_points, vehicle_param_, false); + debug_data_ptr_->msg_stream << " getDebugVisualizationMarker:= " + << stop_watch_.toc("getDebugVisualizationMarker") << " [ms]\n"; + + stop_watch_.tic("publishDebugVisualizationMarker"); + debug_markers_pub_->publish(debug_marker); + debug_data_ptr_->msg_stream << " publishDebugVisualizationMarker:= " + << stop_watch_.toc("publishDebugVisualizationMarker") + << " [ms]\n"; + } + } + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; } Trajectories ObstacleAvoidancePlanner::makePrevTrajectories( - const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const Trajectories & trajs) const + const Trajectories & trajs) { + stop_watch_.tic(__func__); + const auto post_processed_smoothed_traj = - generatePostProcessedTrajectory(ego_pose, path_points, trajs.smoothed_trajectory); + generatePostProcessedTrajectory(path_points, trajs.smoothed_trajectory); + + // 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; - trajectories.extended_trajectory = trajs.extended_trajectory; + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return trajectories; } -Trajectories ObstacleAvoidancePlanner::getBaseTrajectory( +std::vector +ObstacleAvoidancePlanner::generatePostProcessedTrajectory( const std::vector & path_points, - const Trajectories & trajs) const + const std::vector & optimized_traj_points) { - auto base_trajs = trajs; - if (!trajs.extended_trajectory.empty()) { - const auto extended_point_opt = util::getLastExtendedTrajPoint( - path_points.back(), trajs.extended_trajectory.back().pose, - traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_point_opt) { - base_trajs.extended_trajectory.push_back(extended_point_opt.get()); - } - } else if (!trajs.model_predictive_trajectory.empty()) { - const auto extended_point_opt = util::getLastExtendedTrajPoint( - path_points.back(), trajs.model_predictive_trajectory.back().pose, - traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_point_opt) { - base_trajs.extended_trajectory.push_back(extended_point_opt.get()); - } + stop_watch_.tic(__func__); + + std::vector trajectory_points; + if (path_points.empty()) { + autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; + tmp_point.pose = current_ego_pose_; + tmp_point.longitudinal_velocity_mps = 0.0; + trajectory_points.push_back(tmp_point); + return trajectory_points; } - double prev_velocity = 1e4; - for (auto & p : base_trajs.model_predictive_trajectory) { - if (p.longitudinal_velocity_mps < 1e-6) { - p.longitudinal_velocity_mps = prev_velocity; - } else { - prev_velocity = p.longitudinal_velocity_mps; - } + if (optimized_traj_points.empty()) { + trajectory_points = points_utils::convertToTrajectoryPoints(path_points); + return trajectory_points; } - for (auto & p : base_trajs.extended_trajectory) { - if (p.longitudinal_velocity_mps < 1e-6) { - p.longitudinal_velocity_mps = prev_velocity; - } else { - prev_velocity = p.longitudinal_velocity_mps; + + // calculate extended trajectory that connects to optimized trajectory smoothly + const auto extended_traj_points = getExtendedTrajectory(path_points, optimized_traj_points); + + // concat trajectories + const auto full_traj_points = + points_utils::concatTrajectory(optimized_traj_points, extended_traj_points); + + // NOTE: fine_traj_points has no velocity information + const auto fine_traj_points = generateFineTrajectoryPoints(path_points, full_traj_points); + + const auto fine_traj_points_with_vel = + alignVelocity(fine_traj_points, path_points, full_traj_points); + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return fine_traj_points_with_vel; +} + +std::vector +ObstacleAvoidancePlanner::getExtendedTrajectory( + const std::vector & path_points, + const std::vector & optimized_points) +{ + stop_watch_.tic(__func__); + + assert(!path_points.empty()); + + const double accum_arc_length = tier4_autoware_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{}; + } + + // calculate end idx of optimized points on path points + const auto opt_end_path_idx = tier4_autoware_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 { + constexpr double non_fixed_traj_length = 5.0; // TODO(murooka) may be better to tune + 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, non_fixed_traj_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}; } + + // 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_ptr_->extended_non_fixed_traj = interpolated_non_fixed_traj_points; + debug_data_ptr_->extended_fixed_traj = fixed_traj_points; + + return extended_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; } - return base_trajs; + + debug_data_ptr_->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_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + + return interpolated_traj_points; } -boost::optional ObstacleAvoidancePlanner::getStopIdx( +std::vector +ObstacleAvoidancePlanner::alignVelocity( + const std::vector & fine_traj_points, const std::vector & path_points, - const Trajectories & trajs, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & road_clearance_map, DebugData * debug_data) const + const std::vector & traj_points) const { - const int nearest_idx = util::getNearestIdx( - path_points, *current_ego_pose_ptr_, 0, traj_param_->delta_yaw_threshold_for_closest_point); - const double accum_ds = util::getArcLength(path_points, nearest_idx); - - auto target_points = trajs.model_predictive_trajectory; - if (accum_ds < traj_param_->num_sampling_points * traj_param_->delta_arc_length_for_mpt_points) { - target_points.insert( - target_points.end(), trajs.extended_trajectory.begin(), trajs.extended_trajectory.end()); - const auto extended_mpt_point_opt = util::getLastExtendedTrajPoint( - path_points.back(), trajs.model_predictive_trajectory.back().pose, - traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_mpt_point_opt) { - target_points.push_back(extended_mpt_point_opt.get()); + stop_watch_.tic(__func__); + + // search zero velocity index of fine_traj_points + const size_t zero_vel_fine_traj_idx = [&]() { + const size_t zero_vel_path_idx = searchExtendedZeroVelocityIndex(fine_traj_points, path_points); + const size_t zero_vel_traj_idx = + searchExtendedZeroVelocityIndex(fine_traj_points, traj_points); // for outside drivable area + + return std::min(zero_vel_path_idx, zero_vel_traj_idx); + }(); + + auto fine_traj_points_with_vel = fine_traj_points; + size_t prev_begin_idx = 0; + for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { + const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); + + const auto & target_pos = fine_traj_points_with_vel[i].pose.position; + const size_t closest_seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pos); + + // lerp z + fine_traj_points_with_vel[i].pose.position.z = + lerpPoseZ(truncated_points, target_pos, closest_seg_idx); + + // lerp vx + const double target_vel = lerpTwistX(truncated_points, target_pos, closest_seg_idx); + if (i >= zero_vel_fine_traj_idx) { + fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0; + } else if (target_vel < 1e-6) { // NOTE: velocity may be negative due to linear interpolation + const auto prev_idx = std::max(static_cast(i) - 1, 0); + fine_traj_points_with_vel[i].longitudinal_velocity_mps = + fine_traj_points_with_vel[prev_idx].longitudinal_velocity_mps; + } 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_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return fine_traj_points_with_vel; +} + +void ObstacleAvoidancePlanner::publishDebugDataInMain( + const autoware_auto_planning_msgs::msg::Path & path) const +{ + stop_watch_.tic(__func__); + + { // publish trajectories + auto debug_extended_fixed_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->extended_fixed_traj); + debug_extended_fixed_traj.header = path.header; + debug_extended_fixed_traj_pub_->publish(debug_extended_fixed_traj); + + auto debug_extended_non_fixed_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->extended_non_fixed_traj); + debug_extended_non_fixed_traj.header = path.header; + debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); } - const auto footprints = util::getVehicleFootprints(target_points, *vehicle_param_); - const int debug_nearest_footprint_idx = - util::getNearestIdx(target_points, current_ego_pose_ptr_->position); - std::vector debug_truncated_footprints( - footprints.begin() + debug_nearest_footprint_idx, footprints.end()); - debug_data->vehicle_footprints = debug_truncated_footprints; + { // publish clearance map + stop_watch_.tic("publishClearanceMap"); + + if (is_publishing_area_with_objects_) { // false + debug_area_with_objects_pub_->publish(debug_visualization::getDebugCostmap( + debug_data_ptr_->area_with_objects_map, path.drivable_area)); + } + if (is_publishing_object_clearance_map_) { // false + debug_object_clearance_map_pub_->publish(debug_visualization::getDebugCostmap( + debug_data_ptr_->only_object_clearance_map, path.drivable_area)); + } + if (is_publishing_clearance_map_) { // true + debug_clearance_map_pub_->publish( + debug_visualization::getDebugCostmap(debug_data_ptr_->clearance_map, path.drivable_area)); + } + debug_data_ptr_->msg_stream << " getDebugCostMap * 3:= " + << stop_watch_.toc("publishClearanceMap") << " [ms]\n"; + } - const auto optional_idx = - process_cv::getStopIdx(footprints, *current_ego_pose_ptr_, road_clearance_map, map_info); - return optional_idx; + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; } -#include +#include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ObstacleAvoidancePlanner) diff --git a/planning/obstacle_avoidance_planner/src/process_cv.cpp b/planning/obstacle_avoidance_planner/src/process_cv.cpp index 45de56e21804f..f35a9ada665f0 100644 --- a/planning/obstacle_avoidance_planner/src/process_cv.cpp +++ b/planning/obstacle_avoidance_planner/src/process_cv.cpp @@ -14,7 +14,6 @@ #include "obstacle_avoidance_planner/process_cv.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/util.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/util.cpp b/planning/obstacle_avoidance_planner/src/util.cpp deleted file mode 100644 index 527726e5cd801..0000000000000 --- a/planning/obstacle_avoidance_planner/src/util.cpp +++ /dev/null @@ -1,1018 +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/util.hpp" - -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" - -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include -#include - -namespace util -{ -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; -} - -template geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const geometry_msgs::msg::Point &, const geometry_msgs::msg::Pose & origin); -template geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const geometry_msgs::msg::Point32 &, const geometry_msgs::msg::Pose & origin); - -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; -} - -double calculate2DDistance(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) -{ - const double dx = a.x - b.x; - const double dy = a.y - b.y; - return std::sqrt(dx * dx + dy * dy); -} - -double calculateSquaredDistance( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) -{ - const double dx = a.x - b.x; - const double dy = a.y - b.y; - return dx * dx + dy * dy; -} - -double getYawFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) -{ - const double dx = a.x - a_root.x; - const double dy = a.y - a_root.y; - return std::atan2(dy, dx); -} - -geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double yaw) -{ - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - return tf2::toMsg(q); -} - -double normalizeRadian(const double angle) -{ - double n_angle = std::fmod(angle, 2 * M_PI); - n_angle = n_angle > M_PI ? n_angle - 2 * M_PI : n_angle < -M_PI ? 2 * M_PI + n_angle : n_angle; - return n_angle; -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) -{ - const double roll = 0; - const double pitch = 0; - const double yaw = util::getYawFromPoints(a, a_root); - tf2::Quaternion quaternion; - quaternion.setRPY(roll, pitch, yaw); - return tf2::toMsg(quaternion); -} - -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; -} -template geometry_msgs::msg::Point transformMapToImage( - const geometry_msgs::msg::Point &, const nav_msgs::msg::MapMetaData & map_info); -template geometry_msgs::msg::Point transformMapToImage( - const geometry_msgs::msg::Point32 &, const nav_msgs::msg::MapMetaData & map_info); - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & 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; - double image_x = map_y_height - map_y_in_image_resolution; - 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; - } -} - -bool interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - std::vector & interpolated_points) -{ - if (base_x.empty() || base_y.empty()) { - return false; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return false; - } - 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 std::vector interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::slerp(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 false; - } - } - 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 true; -} - -std::vector getInterpolatedPoints( - const std::vector & first_points, - const std::vector & second_points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - std::vector concat_points; - for (const auto point : first_points) { - concat_points.push_back(point.position); - } - for (const auto & point : second_points) { - concat_points.push_back(point.position); - } - - for (std::size_t i = 0; i < concat_points.size(); i++) { - if (i > 0) { - if ( - std::fabs(concat_points[i].x - concat_points[i - 1].x) < 1e-6 && - std::fabs(concat_points[i].y - concat_points[i - 1].y) < 1e-6) { - continue; - } - } - tmp_x.push_back(concat_points[i].x); - tmp_y.push_back(concat_points[i].y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].x - points[i - 1].x) < 1e-6 && - std::fabs(points[i].y - points[i - 1].y) < 1e-6) { - continue; - } - } - tmp_x.push_back(points[i].x); - tmp_y.push_back(points[i].y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].position.x - points[i - 1].position.x) < 1e-6 && - std::fabs(points[i].position.y - points[i - 1].position.y) < 1e-6) { - continue; - } - } - tmp_x.push_back(points[i].position.x); - tmp_y.push_back(points[i].position.y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].p.x - points[i - 1].p.x) < 1e-6 && - std::fabs(points[i].p.y - points[i - 1].p.y) < 1e-6) { - continue; - } - } - tmp_x.push_back(points[i].p.x); - tmp_y.push_back(points[i].p.y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -template -std::vector getInterpolatedPoints( - const T & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::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; - } - } - tmp_x.push_back(points[i].pose.position.x); - tmp_y.push_back(points[i].pose.position.y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} -template std::vector -getInterpolatedPoints>( - const std::vector &, const double); -template std::vector -getInterpolatedPoints>( - const std::vector &, const double); - -template -int getNearestIdx( - const T & points, const geometry_msgs::msg::Pose & pose, const int default_idx, - const double delta_yaw_threshold) -{ - double min_dist = std::numeric_limits::max(); - int nearest_idx = default_idx; - const double point_yaw = tf2::getYaw(pose.orientation); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i].pose.position, pose.position); - const double points_yaw = tf2::getYaw(points[i].pose.orientation); - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::Pose &, const int, const double); -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::Pose &, const int, const double); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const int default_idx, const double delta_yaw_threshold, const double delta_dist_threshold) -{ - int nearest_idx = default_idx; - double min_dist = std::numeric_limits::max(); - const double point_yaw = tf2::getYaw(pose.orientation); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i], pose.position); - double points_yaw = 0; - if (i > 0) { - const double dx = points[i].x - points[i - 1].x; - const double dy = points[i].y - points[i - 1].y; - points_yaw = std::atan2(dy, dx); - } else if (i == 0 && points.size() > 1) { - const double dx = points[i + 1].x - points[i].x; - const double dy = points[i + 1].y - points[i].y; - points_yaw = std::atan2(dy, dx); - } - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if ( - dist < min_dist && dist < delta_dist_threshold && - std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} - -int getNearestIdxOverPoint( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, [[maybe_unused]] const int default_idx, - const double delta_yaw_threshold) -{ - double min_dist = std::numeric_limits::max(); - const double point_yaw = tf2::getYaw(pose.orientation); - const double pose_dx = std::cos(point_yaw); - const double pose_dy = std::sin(point_yaw); - int nearest_idx = 0; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - const double dist = util::calculateSquaredDistance(points[i].pose.position, pose.position); - const double points_yaw = - getYawFromPoints(points[i].pose.position, points[i - 1].pose.position); - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - const double dx = points[i].pose.position.x - pose.position.x; - const double dy = points[i].pose.position.y - pose.position.y; - const double ip = dx * pose_dx + dy * pose_dy; - if (dist < min_dist && ip > 0 && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - } - return nearest_idx; -} - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const int default_idx, const double delta_yaw_threshold) -{ - double min_dist = std::numeric_limits::max(); - int nearest_idx = default_idx; - const double point_yaw = tf2::getYaw(pose.orientation); - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - const double dist = calculateSquaredDistance(points[i], pose.position); - const double points_yaw = getYawFromPoints(points[i], points[i - 1]); - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - } - return nearest_idx; -} - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point, const int default_idx) -{ - double min_dist = std::numeric_limits::max(); - int nearest_idx = default_idx; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - const double dist = calculateSquaredDistance(points[i - 1].pose.position, point); - const double points_dx = points[i].pose.position.x - points[i - 1].pose.position.x; - const double points_dy = points[i].pose.position.y - points[i - 1].pose.position.y; - const double points2pose_dx = point.x - points[i - 1].pose.position.x; - const double points2pose_dy = point.y - points[i - 1].pose.position.y; - const double ip = points_dx * points2pose_dx + points_dy * points2pose_dy; - if (ip < 0) { - return nearest_idx; - } - if (dist < min_dist && ip > 0) { - min_dist = dist; - nearest_idx = i - 1; - } - } - } - return nearest_idx; -} -template int getNearestIdx>( - const std::vector & points, - const geometry_msgs::msg::Point & point, const int default_idx); -template int getNearestIdx>( - const std::vector & points, - const geometry_msgs::msg::Point & point, const int default_idx); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Point & point) -{ - int nearest_idx = 0; - double min_dist = std::numeric_limits::max(); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i], point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point) -{ - int nearest_idx = 0; - double min_dist = std::numeric_limits::max(); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i].pose.position, point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::Point &); -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::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 (std::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; -} - -template -int getNearestPointIdx(const T & points, const geometry_msgs::msg::Point & point) -{ - int nearest_idx = 0; - double min_dist = std::numeric_limits::max(); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i].p, point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} -template int getNearestPointIdx>( - const std::vector &, const geometry_msgs::msg::Point &); -template int getNearestPointIdx>( - const std::vector &, const geometry_msgs::msg::Point &); - -std::vector convertPathToTrajectory( - const std::vector & path_points) -{ - std::vector traj_points; - for (const auto & point : path_points) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; - tmp_point.pose = point.pose; - tmp_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_points.push_back(tmp_point); - } - return traj_points; -} - -std::vector -convertPointsToTrajectoryPointsWithYaw(const std::vector & points) -{ - std::vector traj_points; - for (size_t i = 0; i < points.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose.position = points[i]; - double yaw = 0; - if (i > 0) { - const double dx = points[i].x - points[i - 1].x; - const double dy = points[i].y - points[i - 1].y; - yaw = std::atan2(dy, dx); - } else if (i == 0 && points.size() > 1) { - const double dx = points[i + 1].x - points[i].x; - const double dy = points[i + 1].y - points[i].y; - yaw = std::atan2(dy, dx); - } - const double roll = 0; - const double pitch = 0; - tf2::Quaternion quaternion; - quaternion.setRPY(roll, pitch, yaw); - traj_point.pose.orientation = tf2::toMsg(quaternion); - traj_points.push_back(traj_point); - } - return traj_points; -} - -std::vector fillTrajectoryWithVelocity( - const std::vector & traj_points, - const double velocity) -{ - std::vector traj_with_velocity; - for (const auto & traj_point : traj_points) { - auto tmp_point = traj_point; - tmp_point.longitudinal_velocity_mps = velocity; - traj_with_velocity.push_back(tmp_point); - } - return traj_with_velocity; -} - -template -std::vector alignVelocityWithPoints( - const std::vector & base_traj_points, - const T & points, const int zero_velocity_traj_idx, const int max_skip_comparison_idx) -{ - auto traj_points = base_traj_points; - int prev_begin_idx = 0; - for (std::size_t i = 0; i < traj_points.size(); i++) { - const auto first = points.begin() + prev_begin_idx; - - // TODO(Horibe) could be replaced end() with some reasonable number to reduce computational time - const auto last = points.end(); - const T truncated_points(first, last); - - const size_t closest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(truncated_points, traj_points[i].pose.position); - // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double closest_to_target_dist = tier4_autoware_utils::calcSignedArcLength( - truncated_points, truncated_points.at(closest_seg_idx).pose.position, - traj_points[i].pose.position); - const double seg_dist = tier4_autoware_utils::calcSignedArcLength( - truncated_points, closest_seg_idx, closest_seg_idx + 1); - - // interpolate 1st-nearest (v1) value and 2nd-nearest value (v2) - const auto lerp = [&](const double v1, const double v2, const double ratio) { - return std::abs(seg_dist) < 1e-6 ? v2 : v1 + (v2 - v1) * ratio; - }; - - // z - { - const double closest_z = truncated_points.at(closest_seg_idx).pose.position.z; - const double next_z = truncated_points.at(closest_seg_idx + 1).pose.position.z; - traj_points[i].pose.position.z = lerp(closest_z, next_z, closest_to_target_dist / seg_dist); - } - - // vx - { - const double closest_vel = truncated_points[closest_seg_idx].longitudinal_velocity_mps; - const double next_vel = truncated_points[closest_seg_idx + 1].longitudinal_velocity_mps; - const double target_vel = lerp(closest_vel, next_vel, closest_to_target_dist / seg_dist); - - if (static_cast(i) >= zero_velocity_traj_idx) { - traj_points[i].longitudinal_velocity_mps = 0; - } else if (target_vel < 1e-6) { - const auto idx = std::max(static_cast(i) - 1, 0); - traj_points[i].longitudinal_velocity_mps = traj_points[idx].longitudinal_velocity_mps; - } else if (static_cast(i) <= max_skip_comparison_idx) { - traj_points[i].longitudinal_velocity_mps = target_vel; - } else { - traj_points[i].longitudinal_velocity_mps = - std::fmin(target_vel, traj_points[i].longitudinal_velocity_mps); - } - } - // NOTE: closest_seg_idx is for the clipped trajectory. This operation must be "+=". - prev_begin_idx += closest_seg_idx; - } - return traj_points; -} -template std::vector -alignVelocityWithPoints>( - const std::vector &, - const std::vector &, const int, const int); -template std::vector -alignVelocityWithPoints>( - const std::vector &, - const std::vector &, const int, const int); - -std::vector> getHistogramTable(const std::vector> & input) -{ - std::vector> histogram_table = input; - for (std::size_t i = 0; i < input.size(); i++) { - for (std::size_t j = 0; j < input[i].size(); j++) { - if (input[i][j]) { - histogram_table[i][j] = 0; - } else { - histogram_table[i][j] = (i > 0) ? histogram_table[i - 1][j] + 1 : 1; - } - } - } - return histogram_table; -} - -struct HistogramBin -{ - int height; - int variable_pos; - int original_pos; -}; - -Rectangle getLargestRectangleInRow( - const std::vector & histo, const int current_row, [[maybe_unused]] const int row_size) -{ - std::vector search_histo = histo; - search_histo.push_back(0); - std::stack stack; - Rectangle largest_rect; - for (std::size_t i = 0; i < search_histo.size(); i++) { - HistogramBin bin; - bin.height = search_histo[i]; - bin.variable_pos = i; - bin.original_pos = i; - if (stack.empty()) { - stack.push(bin); - } else { - if (stack.top().height < bin.height) { - stack.push(bin); - } else if (stack.top().height >= bin.height) { - int target_i = i; - while (!stack.empty() && bin.height <= stack.top().height) { - HistogramBin tmp_bin = stack.top(); - stack.pop(); - int area = (i - tmp_bin.variable_pos) * tmp_bin.height; - if (area > largest_rect.area) { - largest_rect.max_y_idx = tmp_bin.variable_pos; - largest_rect.min_y_idx = i - 1; - largest_rect.max_x_idx = current_row - tmp_bin.height + 1; - largest_rect.min_x_idx = current_row; - largest_rect.area = area; - } - - target_i = tmp_bin.variable_pos; - } - bin.variable_pos = target_i; - stack.push(bin); - } - } - } - return largest_rect; -} - -Rectangle getLargestRectangle(const std::vector> & input) -{ - std::vector> histogram_table = getHistogramTable(input); - Rectangle largest_rectangle; - for (std::size_t i = 0; i < histogram_table.size(); i++) { - Rectangle rect = getLargestRectangleInRow(histogram_table[i], i, input.size()); - if (rect.area > largest_rectangle.area) { - largest_rectangle = rect; - } - } - return largest_rectangle; -} - -boost::optional getLastExtendedPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold) -{ - const double dist = calculate2DDistance(path_point.pose.position, pose.position); - const double diff_yaw = tf2::getYaw(path_point.pose.orientation) - tf2::getYaw(pose.orientation); - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if ( - dist > 1e-6 && dist < delta_dist_threshold && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - return path_point.pose.position; - } else { - return boost::none; - } -} - -boost::optional getLastExtendedTrajPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold) -{ - const double dist = calculate2DDistance(path_point.pose.position, pose.position); - const double diff_yaw = tf2::getYaw(path_point.pose.orientation) - tf2::getYaw(pose.orientation); - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if ( - dist > 1e-6 && dist < delta_dist_threshold && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_traj_p; - tmp_traj_p.pose.position = path_point.pose.position; - tmp_traj_p.pose.orientation = - util::getQuaternionFromPoints(path_point.pose.position, pose.position); - tmp_traj_p.longitudinal_velocity_mps = path_point.longitudinal_velocity_mps; - return tmp_traj_p; - } else { - return boost::none; - } -} - -std::vector getVehicleFootprints( - const std::vector & optimized_points, - const VehicleParam & vehicle_param) -{ - const double baselink_to_top = vehicle_param.length - vehicle_param.rear_overhang; - const double half_width = vehicle_param.width * 0.5; - std::vector rel_lon_offset{baselink_to_top, baselink_to_top, 0, 0}; - std::vector rel_lat_offset{half_width, -half_width, -half_width, half_width}; - std::vector rects; - for (std::size_t i = 0; i < optimized_points.size(); i++) { - // for (int i = nearest_idx; i < optimized_points.size(); i++) { - std::vector abs_points; - for (std::size_t j = 0; j < rel_lon_offset.size(); j++) { - geometry_msgs::msg::Point rel_point; - rel_point.x = rel_lon_offset[j]; - rel_point.y = rel_lat_offset[j]; - abs_points.push_back( - util::transformToAbsoluteCoordinate2D(rel_point, optimized_points[i].pose)); - } - Footprint rect; - rect.p = optimized_points[i].pose.position; - rect.top_left = abs_points[0]; - rect.top_right = abs_points[1]; - rect.bottom_right = abs_points[2]; - rect.bottom_left = abs_points[3]; - rects.push_back(rect); - } - return rects; -} - -/* - * calculate distance in x-y 2D space - */ -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; -} - -bool hasValidNearestPointFromEgo( - const geometry_msgs::msg::Pose & ego_pose, const Trajectories & trajs, - const TrajectoryParam & traj_param) -{ - const auto traj = concatTraj(trajs); - const auto interpolated_points = - util::getInterpolatedPoints(traj, traj_param.delta_arc_length_for_trajectory); - const int default_idx = -1; - const int nearest_idx_with_thres = util::getNearestIdx( - interpolated_points, ego_pose, default_idx, traj_param.delta_yaw_threshold_for_closest_point, - traj_param.delta_dist_threshold_for_closest_point); - if (nearest_idx_with_thres == default_idx) { - return false; - } - return true; -} - -std::vector concatTraj( - const Trajectories & trajs) -{ - std::vector trajectory; - trajectory.insert( - trajectory.end(), trajs.model_predictive_trajectory.begin(), - trajs.model_predictive_trajectory.end()); - trajectory.insert( - trajectory.end(), trajs.extended_trajectory.begin(), trajs.extended_trajectory.end()); - return trajectory; -} - -int getZeroVelocityIdx( - const bool is_showing_debug_info, const std::vector & fine_points, - const std::vector & path_points, - const std::unique_ptr & opt_trajs, const TrajectoryParam & traj_param) -{ - const int default_idx = fine_points.size() - 1; - const int zero_velocity_idx_from_path = - getZeroVelocityIdxFromPoints(path_points, fine_points, default_idx, traj_param); - - int zero_velocity_idx_from_opt_points = zero_velocity_idx_from_path; - if (opt_trajs) { - zero_velocity_idx_from_opt_points = getZeroVelocityIdxFromPoints( - util::concatTraj(*opt_trajs), fine_points, default_idx, traj_param); - } - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("util"), is_showing_debug_info, - "0 m/s idx from path: %d from opt: %d total size %zu", zero_velocity_idx_from_path, - zero_velocity_idx_from_opt_points, fine_points.size()); - const int zero_velocity_idx = - std::min(zero_velocity_idx_from_path, zero_velocity_idx_from_opt_points); - - if (is_showing_debug_info) { - int zero_velocity_path_idx = path_points.size() - 1; - for (std::size_t i = 0; i < path_points.size(); i++) { - if (path_points[i].longitudinal_velocity_mps < 1e-6) { - zero_velocity_path_idx = i; - break; - } - } - const float debug_dist = util::calculate2DDistance( - path_points[zero_velocity_path_idx].pose.position, fine_points[zero_velocity_idx]); - RCLCPP_INFO( - rclcpp::get_logger("util"), "Dist from path 0 velocity point: = %f [m]", debug_dist); - } - return zero_velocity_idx; -} - -template -int getZeroVelocityIdxFromPoints( - const T & points, const std::vector & fine_points, - const int /* default_idx */, const TrajectoryParam & traj_param) -{ - int zero_velocity_points_idx = points.size() - 1; - for (std::size_t i = 0; i < points.size(); i++) { - if (points[i].longitudinal_velocity_mps < 1e-6) { - zero_velocity_points_idx = i; - break; - } - } - const int default_zero_velocity_fine_points_idx = fine_points.size() - 1; - const int zero_velocity_fine_points_idx = util::getNearestIdx( - fine_points, points[zero_velocity_points_idx].pose, default_zero_velocity_fine_points_idx, - traj_param.delta_yaw_threshold_for_closest_point, - traj_param.delta_dist_threshold_for_closest_point); - return zero_velocity_fine_points_idx; -} -template int getZeroVelocityIdxFromPoints>( - const std::vector &, - const std::vector &, const int, const TrajectoryParam &); -template int -getZeroVelocityIdxFromPoints>( - const std::vector &, - const std::vector &, const int, const TrajectoryParam &); - -template -double getArcLength(const T & points, const int initial_idx) -{ - double accum_arc_length = 0; - for (size_t i = initial_idx; i < points.size(); i++) { - if (i > 0) { - const double dist = - util::calculate2DDistance(points[i].pose.position, points[i - 1].pose.position); - accum_arc_length += dist; - } - } - return accum_arc_length; -} -template double getArcLength>( - const std::vector &, const int); -template double getArcLength>( - const std::vector &, const int); - -double getArcLength(const std::vector & points, const int initial_idx) -{ - double accum_arc_length = 0; - for (size_t i = initial_idx; i < points.size(); i++) { - if (i > 0) { - const double dist = util::calculate2DDistance(points[i].position, points[i - 1].position); - accum_arc_length += dist; - } - } - return accum_arc_length; -} - -void logOSQPSolutionStatus(const int solution_status) -{ - /****************** - * Solver Status * - ******************/ - int OSQP_DUAL_INFEASIBLE_INACCURATE = 4; - int OSQP_PRIMAL_INFEASIBLE_INACCURATE = 3; - int OSQP_SOLVED_INACCURATE = 2; - int OSQP_SOLVED = 1; - int OSQP_MAX_ITER_REACHED = -2; - int OSQP_PRIMAL_INFEASIBLE = -3; /* primal infeasible */ - int OSQP_DUAL_INFEASIBLE = -4; /* dual infeasible */ - int OSQP_SIGINT = -5; /* interrupted by user */ - - if (solution_status == OSQP_SOLVED) { - // RCLCPP_INFO(rclcpp::get_logger("util"),"[Avoidance] OSQP solution status: OSQP_SOLVED"); - } else if (solution_status == OSQP_DUAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE"); - } else if (solution_status == OSQP_PRIMAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE"); - } else if (solution_status == OSQP_SOLVED_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SOLVED_INACCURATE"); - } else if (solution_status == OSQP_MAX_ITER_REACHED) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_ITER_REACHED"); - } else if (solution_status == OSQP_PRIMAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE"); - } else if (solution_status == OSQP_DUAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE"); - } else if (solution_status == OSQP_SIGINT) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SIGINT"); - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] Interrupted by user, process will be finished."); - std::exit(0); - } else { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: Not defined %d", - solution_status); - } -} - -} // namespace util diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp new file mode 100644 index 0000000000000..3ae8832531da5 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -0,0 +1,551 @@ +// 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.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 +{ +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 slerpTwoPoints( + 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::slerp(base_s, base_x, new_s); + const std::vector interpolated_y = interpolation::slerp(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 = + slerpTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); + const auto interpolated_y = + slerpTwoPoints(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::slerp(base_s, base_x, new_s); + const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); + const auto interpolated_yaw = interpolation::slerp(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 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), validated_pose.at(2), 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) +{ + /****************** + * 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] OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE"); + } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), + "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE"); + } else if (solution_status == LOCAL_OSQP_SOLVED_INACCURATE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SOLVED_INACCURATE"); + } else if (solution_status == LOCAL_OSQP_MAX_ITER_REACHED) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_ITER_REACHED"); + } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE"); + } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE"); + } else if (solution_status == LOCAL_OSQP_SIGINT) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SIGINT"); + 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] OSQP solution status: OSQP_TIME_LIMIT_REACHED"); + } else if (solution_status == LOCAL_OSQP_UNSOLVED) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_UNSOLVED"); + } else if (solution_status == LOCAL_OSQP_NON_CVX) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_NON_CVX"); + } else { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: Not defined %d", + solution_status); + } +} +} // namespace 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 d842036dd6f97..e752c31f242da 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 @@ -15,48 +15,53 @@ #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include +#include +#include -KinematicsBicycleModel::KinematicsBicycleModel( - const double wheelbase, const double steer_lim, const double steer_tau) -: VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2) +KinematicsBicycleModel::KinematicsBicycleModel(const double wheel_base, const double steer_limit) +: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheel_base, steer_limit) { - wheelbase_ = wheelbase; - steer_lim_ = steer_lim; - steer_tau_ = steer_tau; } -void KinematicsBicycleModel::calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) +void KinematicsBicycleModel::calculateStateEquationMatrix( + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) { - auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; + // const double epsilon = std::numeric_limits::epsilon(); + // constexpr double dt = 0.03; // assuming delta time for steer tau - /* 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)); + /* + const double lf = wheel_base_ - center_offset_from_base_; + const double lr = center_offset_from_base_; + */ - // Ad << 0.0, velocity, 0.0, 0.0, 0.0, velocity / wheelbase_ * cos_delta_r_squared_inv, 0.0, 0.0, - // -1.0 / steer_tau_; - // Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_x_, dim_x_); - // Ad = (I - dt * 0.5 * Ad).inverse() * (I + dt * 0.5 * Ad); // bilinear discretization + const double delta_r = std::atan(wheel_base_ * curvature_); + const double cropped_delta_r = std::clamp(delta_r, -steer_limit_, steer_limit_); - // assuming delta time for steer tau - constexpr double dt = 0.03; - *Ad << 1.0, ds, 0, 0.0, 1, ds / (wheelbase_ * cos_delta_r_squared_inv), 0.0, 0, - 1 - dt / steer_tau_; + // 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, 0.0, dt / steer_tau_; + Bd << 0.0, ds / wheel_base_ / std::pow(std::cos(delta_r), 2.0); - *Cd << 1.0, 0.0, 0.0, 0.0, 1.0, 0.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)); +} - *Wd << 0.0, - -ds * curvature_ + ds / wheelbase_ * (tan(delta_r) - delta_r * cos_delta_r_squared_inv), 0.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(wheelbase_ * curvature_); + (*Uref)(0, 0) = std::atan(wheel_base_ * 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 50a2d72f841a6..8e6522725c133 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 @@ -14,11 +14,24 @@ #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" -VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y) -: dim_x_(dim_x), dim_u_(dim_u), dim_y_(dim_y) +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) { } + 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; }